【激光雷达点云障碍物检测】render.h、render.cpp、box.h、kdtree3d.h、lidar.h

 render.h 文件中实现和构建了用于渲染环境使用的函数和结构体

// Functions and structs used to render the enviroment 用于环境渲染的函数和结构体
// such as cars and the highway

#ifndef RENDER_H
#define RENDER_H
//#endif
//以上三行的定义 是为了避免头文件的重复使用

#include <pcl/visualization/pcl_visualizer.h>
#include "box.h"
#include <iostream>
#include <vector>
#include <string>

struct Color  //结构体中使用构造函数初始化列表
{
	float r, g, b;
	//Color(float setR, float setG, float setB): r(setR), g(setG), b(setB){}  //构造函数初始化列表
	Color(float setR, float setG, float setB)    //含有参数的构造函数，以便创建Color变量时不向其传递参数时，提供默认值
	{
		r = setR;
		g = setG;
		b = setB;
	}
};

struct Vect3
{
	double x, y, z;
	Vect3(double setX, double setY, double setZ)  //构造函数初始化列表
		: x(setX), y(setY), z(setZ)
	{}
	Vect3 operator+(const Vect3& vec)  //将结构体传递给函数
	{
		Vect3 result(x + vec.x, y + vec.y, z + vec.z);
		return result;
	}
};

enum CameraAngle //枚举类型
{
	XY, TopDown, Side, FPS
};

struct Car
{
	// 变量 position （位置）和 dimensions （尺寸大小）两个变量中的xyz的单位为米
	Vect3 position, dimensions;

	std::string name;
	Color color;

	//构造函数初始化列表
	Car(Vect3 setPosition, Vect3 setDimensions, Color setColor, std::string setName)
		: position(setPosition), dimensions(setDimensions), color(setColor), name(setName)
	{}

	void render(pcl::visualization::PCLVisualizer::Ptr& viewer)
	{
		// render bottom of car  车辆底部的渲染
		//viewer->addCube 向视图中添加一个立方体模型 
		/*
		bool pcl::visualization::PCLVisualizer::addCube  ( float  x_min,  
					float  x_max,  
					float  y_min,  
					float  y_max,  
					float  z_min,  
					float  z_max,  
					double  r = 1.0,  
					double  g = 1.0,  
					double  b = 1.0,  
					const std::string &  id = "cube",  
					int  viewport = 0  
					) 
		*/
		viewer->addCube(position.x - dimensions.x / 2, position.x + dimensions.x / 2, position.y - dimensions.y / 2, position.y + dimensions.y / 2, position.z, position.z + dimensions.z * 2 / 3, color.r, color.g, color.b, name);
		// setShapeRenderingProperties 设置格子的属性
		viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name);
		viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
		viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, name);
		// render top of car  车辆顶部的渲染
		viewer->addCube(position.x - dimensions.x / 4, position.x + dimensions.x / 4, position.y - dimensions.y / 2, position.y + dimensions.y / 2, position.z + dimensions.z * 2 / 3, position.z + dimensions.z, color.r, color.g, color.b, name + "Top");
		viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name + "Top");
		viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name + "Top");
		viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, name + "Top");
	}

	// collision helper function  初步猜想：检测是否车辆与周围点碰撞
	bool inbetween(double point, double center, double range)
	{
		return (center - range <= point) && (center + range >= point);
	}
	bool checkCollision(Vect3 point)
	{
		return (inbetween(point.x, position.x, dimensions.x / 2) && inbetween(point.y, position.y, dimensions.y / 2) && inbetween(point.z, position.z + dimensions.z / 3, dimensions.z / 3)) ||
			(inbetween(point.x, position.x, dimensions.x / 4) && inbetween(point.y, position.y, dimensions.y / 2) && inbetween(point.z, position.z + dimensions.z * 5 / 6, dimensions.z / 6));

	}
};
//函数实现功能待定
void renderHighway(pcl::visualization::PCLVisualizer::Ptr& viewer);
void renderRays(pcl::visualization::PCLVisualizer::Ptr& viewer, const Vect3& origin, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
void clearRays(pcl::visualization::PCLVisualizer::Ptr& viewer);
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color = Color(1, 1, 1));
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color = Color(-1, -1, -1));
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, Box box, int id, Color color = Color(1, 0, 0), float opacity = 1);
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, BoxQ box, int id, Color color = Color(1, 0, 0), float opacity = 1);

#endif

render.cpp

/* \author Aaron Brown */
// Functions and structs used to render the enviroment
// such as cars and the highway

#include "render.h"

void renderHighway(pcl::visualization::PCLVisualizer::Ptr& viewer)
{

	// units in meters
	double roadLength = 50.0;
	double roadWidth = 12.0;
	double roadHeight = 0.2;

	viewer->addCube(-roadLength / 2, roadLength / 2, -roadWidth / 2, roadWidth / 2, -roadHeight, 0, .2, .2, .2, "highwayPavement");
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, "highwayPavement");
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, .2, .2, .2, "highwayPavement");
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, "highwayPavement");
	viewer->addLine(pcl::PointXYZ(-roadLength / 2, -roadWidth / 6, 0.01), pcl::PointXYZ(roadLength / 2, -roadWidth / 6, 0.01), 1, 1, 0, "line1");
	viewer->addLine(pcl::PointXYZ(-roadLength / 2, roadWidth / 6, 0.01), pcl::PointXYZ(roadLength / 2, roadWidth / 6, 0.01), 1, 1, 0, "line2");
}

int countRays = 0;
void renderRays(pcl::visualization::PCLVisualizer::Ptr& viewer, const Vect3& origin, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{

	for (pcl::PointXYZ point : cloud->points)
	{
		viewer->addLine(pcl::PointXYZ(origin.x, origin.y, origin.z), point, 1, 0, 0, "ray" + std::to_string(countRays));
		countRays++;
	}
}

void clearRays(pcl::visualization::PCLVisualizer::Ptr& viewer)
{
	while (countRays)
	{
		countRays--;
		viewer->removeShape("ray" + std::to_string(countRays));
	}
}

//选取需要渲染的点云的种类 障碍物 地面 全部点云
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color)
{

	viewer->addPointCloud<pcl::PointXYZ>(cloud, name);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
}

void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color)
{

	if (color.r == -1)
	{
		// Select color based off of cloud intensity
		pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> intensity_distribution(cloud, "intensity");
		viewer->addPointCloud<pcl::PointXYZI>(cloud, intensity_distribution, name);
	}
	else
	{
		// Select color based off input value
		viewer->addPointCloud<pcl::PointXYZI>(cloud, name);
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
	}

	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
}

// Draw wire frame box with filled transparent color 
// renderBox(viewer, box, clusterId);  对每个点云画框
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, Box box, int id, Color color, float opacity)
{
	if (opacity > 1.0)
		opacity = 1.0;
	if (opacity < 0.0)
		opacity = 0.0;

	std::string cube = "box" + std::to_string(id);
	//viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cube);
	viewer->addCube(box.x_min, box.x_max, box.y_min, box.y_max, box.z_min, box.z_max, color.r, color.g, color.b, cube);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, cube);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cube);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cube);

	std::string cubeFill = "boxFill" + std::to_string(id);
	//viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cubeFill);
	viewer->addCube(box.x_min, box.x_max, box.y_min, box.y_max, box.z_min, box.z_max, color.r, color.g, color.b, cubeFill);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, cubeFill);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cubeFill);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity*0.3, cubeFill);
}

//这个函数与上个函数重复
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, BoxQ box, int id, Color color, float opacity)
{
	if (opacity > 1.0)
		opacity = 1.0;
	if (opacity < 0.0)
		opacity = 0.0;

	std::string cube = "box" + std::to_string(id);
	viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cube);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, cube);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cube);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cube);

	std::string cubeFill = "boxFill" + std::to_string(id);
	viewer->addCube(box.bboxTransform, box.bboxQuaternion, box.cube_length, box.cube_width, box.cube_height, cubeFill);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, cubeFill);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, cubeFill);
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity*0.3, cubeFill);
}

box.h

#ifndef BOX_H
#define BOX_H

#include <Eigen/Geometry> 
struct BoxQ
{
	Eigen::Vector3f bboxTransform;  // Vector3f 单精度的xyz坐标 与之对应的Vector3D双精度，更加精确，但运行速度也会慢
	//浮点型的四元数  Quaternion (const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)
	/*
	四元数都是由实部w 加上三个虚部 x、y、z 组成
	四元数一般可表示为a + bx+ cy + dz，其中a、b、c 、d是实数
	*/
	Eigen::Quaternionf bboxQuaternion;
	float cube_length;
	float cube_width;
	float cube_height;
};
struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
};

#endif

kdtree3d.h

#ifndef PLAYBACK_KDTREE3D_H
#define PLAYBACK_KDTREE3D_H

#include <pcl/impl/point_types.hpp>
#include <vector>

struct Node {
	pcl::PointXYZI point;
	int id;
	Node *left;
	Node *right;
	Node(pcl::PointXYZI arr, int setId) : point(arr), id(setId), left(NULL), right(NULL) {}
};

struct KdTree {
	Node *root;

	KdTree() : root(NULL) {}

	void insertHelper(Node **node, int depth, pcl::PointXYZI point, int id) {
		// Tree is empty
		if (*node == NULL) {
			*node = new Node{ point, id };
		}
		else {
			// calculate current din
			int cd = depth % 3;
			if (cd == 0) {
				if (point.x < (*node)->point.x) {
					insertHelper(&((*node)->left), depth + 1, point, id);
				}
				else {
					insertHelper(&((*node)->right), depth + 1, point, id);
				}
			}
			else if (cd == 1) {
				if (point.y < (*node)->point.y) {
					insertHelper(&((*node)->left), depth + 1, point, id);
				}
				else {
					insertHelper(&((*node)->right), depth + 1, point, id);
				}
			}
			else {
				if (point.z < (*node)->point.z) {
					insertHelper(&((*node)->left), depth + 1, point, id);
				}
				else {
					insertHelper(&((*node)->right), depth + 1, point, id);
				}
			}
		}
	}

	void insert(pcl::PointXYZI point, int id) {
		// the function should create a new node and place correctly with in the root
		insertHelper(&root, 0, point, id);
	}

	void searchHelper(pcl::PointXYZI target, Node *node, int depth, float distanceTol, std::vector<int> &ids) {
		if (node != NULL) {
			float delta_x = node->point.x - target.x;
			float delta_y = node->point.y - target.y;
			float delta_z = node->point.z - target.z;

			if ((delta_x >= -distanceTol && delta_x <= distanceTol) &&
				(delta_y >= -distanceTol && delta_y <= distanceTol) &&
				(delta_z >= -distanceTol && delta_z <= distanceTol)) {
				float distance = sqrt(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z);
				if (distance <= distanceTol) {
					ids.push_back(node->id);
				}
			}
			// check across boundary
			if (depth % 3 == 0) {
				if (delta_x > -distanceTol) {
					searchHelper(target, node->left, depth + 1, distanceTol, ids);
				}
				if (delta_x < distanceTol) {
					searchHelper(target, node->right, depth + 1, distanceTol, ids);
				}
			}
			else if (depth % 3 == 1) {
				if (delta_y > -distanceTol) {
					searchHelper(target, node->left, depth + 1, distanceTol, ids);
				}
				if (delta_y < distanceTol) {
					searchHelper(target, node->right, depth + 1, distanceTol, ids);
				}
			}
			else {
				if (delta_z > -distanceTol) {
					searchHelper(target, node->left, depth + 1, distanceTol, ids);
				}
				if (delta_z < distanceTol) {
					searchHelper(target, node->right, depth + 1, distanceTol, ids);
				}
			}

		}
	}
	// return a list of point ids in the tree that are within distance of target
	std::vector<int> search(pcl::PointXYZI target, float distanceTol)
	{
		std::vector<int> ids;
		searchHelper(target, root, 0, distanceTol, ids);
		return ids;
	}

};


#endif //PLAYBACK_KDTREE3D_H

lidar.h

#ifndef LIDAR_H
#define LIDAR_H
#include "render.h"
#include <ctime>
#include <chrono>

const double pi = 3.1415;

struct Ray
{

	Vect3 origin;
	double resolution;
	Vect3 direction;
	Vect3 castPosition;
	double castDistance;

	// parameters:
	// setOrigin: the starting position from where the ray is cast
	// horizontalAngle: the angle of direction the ray travels on the xy plane
	// verticalAngle: the angle of direction between xy plane and ray 
	// 				  for example 0 radians is along xy plane and pi/2 radians is stright up
	// resoultion: the magnitude of the ray's step, used for ray casting, the smaller the more accurate but the more expensive

	Ray(Vect3 setOrigin, double horizontalAngle, double verticalAngle, double setResolution)
		: origin(setOrigin), resolution(setResolution), direction(resolution*cos(verticalAngle)*cos(horizontalAngle), resolution*cos(verticalAngle)*sin(horizontalAngle), resolution*sin(verticalAngle)),
		castPosition(origin), castDistance(0)
	{}

	void rayCast(const std::vector<Car>& cars, double minDistance, double maxDistance, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, double slopeAngle, double sderr)
	{
		// reset ray
		castPosition = origin;
		castDistance = 0;

		bool collision = false;

		while (!collision && castDistance < maxDistance)
		{

			castPosition = castPosition + direction;
			castDistance += resolution;

			// check if there is any collisions with ground slope
			collision = (castPosition.z <= castPosition.x * tan(slopeAngle));

			// check if there is any collisions with cars
			if (!collision && castDistance < maxDistance)
			{
				for (Car car : cars)
				{
					collision |= car.checkCollision(castPosition);
					if (collision)
						break;
				}
			}
		}

		if ((castDistance >= minDistance) && (castDistance <= maxDistance))
		{
			// add noise based on standard deviation error
			double rx = ((double)rand() / (RAND_MAX));
			double ry = ((double)rand() / (RAND_MAX));
			double rz = ((double)rand() / (RAND_MAX));
			cloud->points.push_back(pcl::PointXYZ(castPosition.x + rx*sderr, castPosition.y + ry*sderr, castPosition.z + rz*sderr));
		}

	}

};

struct Lidar
{

	std::vector<Ray> rays;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
	std::vector<Car> cars;
	Vect3 position;
	double groundSlope;
	double minDistance;
	double maxDistance;
	double resoultion;
	double sderr;

	Lidar(std::vector<Car> setCars, double setGroundSlope)
		: cloud(new pcl::PointCloud<pcl::PointXYZ>()), position(0, 0, 2.6)
	{
		// TODO:: set minDistance to 5 to remove points from roof of ego car
		minDistance = 5;
		maxDistance = 50;
		resoultion = 0.2;
		// TODO:: set sderr to 0.2 to get more interesting pcd files
		sderr = 0.2;
		cars = setCars;
		groundSlope = setGroundSlope;

		// TODO:: increase number of layers to 8 to get higher resoultion pcd
		int numLayers = 8;
		// the steepest vertical angle
		double steepestAngle = 30.0*(-pi / 180);
		double angleRange = 26.0*(pi / 180);
		// TODO:: set to pi/64 to get higher resoultion pcd
		double horizontalAngleInc = pi / 64;

		double angleIncrement = angleRange / numLayers;

		for (double angleVertical = steepestAngle; angleVertical < steepestAngle + angleRange; angleVertical += angleIncrement)
		{
			for (double angle = 0; angle <= 2 * pi; angle += horizontalAngleInc)
			{
				Ray ray(position, angle, angleVertical, resoultion);
				rays.push_back(ray);
			}
		}
	}

	~Lidar()
	{
		// pcl uses boost smart pointers for cloud pointer so we don't have to worry about manually freeing the memory
	}

	pcl::PointCloud<pcl::PointXYZ>::Ptr scan()
	{
		cloud->points.clear();
		auto startTime = std::chrono::steady_clock::now();
		for (Ray ray : rays)
			ray.rayCast(cars, minDistance, maxDistance, cloud, groundSlope, sderr);
		auto endTime = std::chrono::steady_clock::now();
		auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
		cout << "ray casting took " << elapsedTime.count() << " milliseconds" << endl;
		cloud->width = cloud->points.size();
		cloud->height = 1; // one dimensional unorganized point cloud dataset
		return cloud;
	}

};

#endif