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