[游戏开发]robosense16线激光雷达栅格法建图实例

一、栅格法建图原理
激光点云栅格化核心思想是将激光雷达所扫描到的区域用网格进行处理，每个栅格点云代表空间的一小块区域，内含一部分点云，点云栅格化处理分为二维栅格化和三维栅格化，二维其实就是将三维点云进行一个投影。不考虑ｚ值的变化。
这里我们先讲一下二维栅格化的处理：
?　 我们假设地面相对平坦，即地面扫描点的 z 轴方向的波动较小，通过将扫描区域进行栅格划分，将扫描点云投影到 xy 的栅格平面，通过统计栅格中 z 轴方面的最高点和最低点的差值(即，极差)，判断栅格中的点是否为地面点或障碍物点。

二、代码分析
1.首先是建立一个3D激光雷达线程

pthread_t g_Thread3DL0Handle;
pthread_create(&g_Thread3DL0Handle, NULL, Thread3D_L0, &gParm);//线程的四个参数分别是线程标志位，线程属性，线程入口函数和线程函数参数
pthread_join(g_Thread3DHandle, NULL);

2.其次建立两个线程：接收雷达数据和处理雷达数据

void *Thread3D_L0(void *args)
{
	int rt;
	pthread_t lidar_robosense_recv;
	pthread_t lidar_robosense_handle;
	rt = ntzx_lidar_robosense_init();
	if(rt < 0)
	{
		printf("ntzx_lidar_robosense_init error\n")；
		return NULL;
	}
	/*创建两个线程，接收线程与处理线程*/
	rt = pthread_create(&lidar_robosense_recv,NULL,ntzx_lidar_robosense_recv,&gParm);
	if(rt < 0)
    {
		printf("ntzx_lidar_robosense_recv error\n");
		return NULL;
	}
	rt = pthread_create(&lidar_robosense_handle,NULL,ntzx_lidar_robosense_handle,&gParm);
    if(rt < 0)
	{
		printf("ntzx_lidar_robosense_handle error\n");
		return NULL;
	}
	pthread_join(lidar_robosense_recv, NULL);
    pthread_join(lidar_robosense_handle, NULL);
    return NULL;
}
void *ntzx_lidar_robosense_recv(void *recvbuf)
{
    WRC_3D16_L0_UDPClient * pUdp = (WRC_3D16_L0_UDPClient*)recvbuf;//雷达数据接收线程参数强制转化为WRC_3D16_L0_UDPClient类，将它赋给这个类的对象
	pUdp->ReceivingLoop();
	return recvbuf;
}

void *ntzx_lidar_robosense_handle(void *handlebuf)
{
		WRC_3D16_L0_UDPClient * pUdp = (WRC_3D16_L0_UDPClient*)handlebuf;
	//(pUdp->My_DataProc).pDataProc_param=(WRC_3D16_L0_UDPClient*)lparam;
	if((pUdp->My_DataProc).DataProc_Initialize())
        (pUdp->My_DataProc).ProcessingLoop();
	return handlebuf;
}

解析一下WRC_3D16_L0_UDPClient *pUdp是什么？
它就是一个类指针，就相当于结构体指针，它的形式是这样的：

class WRC_3D16_L0_UDPClient
{
public:
	WRC_3D16_L0_UDPClient();
	~WRC_3D16_L0_UDPClient();

	void  ReceivingLoop();//接收UDP包函数
	int   Open();//初始化UDP连接、开启两个线程

	WRC_3D16_L0_DataProc My_DataProc;//WRC_3D16_L0_DataProc类的对象
    pthread_t  m_RecvThreadHandle; //接收线程ID
	pthread_t  m_ProcThreadHandle;//处理线程ID
	bool Is_Init; //是否初始化连接过，若没有 必须先初始化


private:
	int L0_UdpSocket; // UDP通讯套接字
	unsigned char  UdpPacketBuf[MAX_UDP_PACKET_SIZE];//缓冲区

private:
	void  ConnectLidar(uint16_t port);//创建链接
	int   CreateTwoTHREAD();//创建接收数据和处理数据线程
	bool  IsPacketHeader(unsigned char *buff);//根据校验头判断是否为要接受的数据


};

三、循环接收雷达传来的数据包

void  WRC_3D16_L0_UDPClient::ReceivingLoop()
{
	sockaddr_in server_addr;//建立socket通信的地址
	socklen_t server_len;//地址的长度
	int PacketLen;//正确接收的数据包中的字节数
	int idx;
	int SleepTime;//连续睡眠时间计数

	SleepTime = 0;
	while (Is_Init)
	{
		//step.1---------------接收一个网络包-------------------------//
		server_len = sizeof(server_addr);//地址的长度
		PacketLen = recvfrom(L0_UdpSocket, (unsigned char *)UdpPacketBuf, MAX_UDP_PACKET_SIZE, 0,(struct sockaddr*) &server_addr, &server_len);
        //开始接收激光雷达数据包，返回正确接收数据包中的字节数。参数分别是UDP通信套接字，定义的一个存放接收数据的数组，存放的最大字节长度，调用长度，指向源地址的缓冲区，缓冲区长度
		//作用：从（已连接）套接口上接收数据，并捕获数据发送源的地址
		//step.2---------------存放本包数据---------------------------//
		if (PacketLen == UDP_PACKET_SIZE && IsPacketHeader(UdpPacketBuf))//如果接收到的数据长度==1248以及将接收到的数据进行数据包头判断
		{
			SleepTime = 0;//睡眠计数清零
			pthread_mutex_lock(&g_L0_Mutex_NewPacket);
			idx =g_L0_ReceivedPacketNum % MAX_ONE_FRAME_PACKET;//用接收到的多少UDP数据包%接收到的UDP包，可以得到一帧数据有多少数据包
			memcpy(&(g_L0_OneFrameBuf[idx][0]), UdpPacketBuf, PacketLen);//将接收到的数据放入第一个数据包
			g_L0_OneFramePacketSize[idx] = PacketLen;
			g_L0_ReceivedPacketNum++;
			pthread_mutex_unlock(&g_L0_Mutex_NewPacket);
		}
		//step.3---------------稍等,取下一包--------------------------//
		else
		{
			usleep(PACKET_INTERVAL_TIME);
			SleepTime++;
			if (SleepTime > 100) {
				Is_Init = false;
				//OUTPUT_LOG("3D:L0:==== Can't Receive Data，Initialize and getPacket again!===\n");
				printf("3D:L0:==== Can't Receive Data，Initialize and getPacket again!===\n");
				ConnectLidar(PORT_NUMBER_L0);
				if(Is_Init)
					ReceivingLoop();
			}
		}
	}

	//OUTPUT_LOG("3D:L0:getPacket done!\n");
	printf("3D:L0:getPacket done!\n");
	return;

}

四、激光雷达坐标系和车辆坐标系之间的转换
利用激光雷达测光和测距来计算我们关心的物体的精确距离。

激光雷达利用飞行时间计算物体的距离
当发射激光脉冲时，其发射时间和方向将被记录。激光脉冲在空气中传播，直到它碰到一个能反射一些能量的障碍物。在接收到能量的部分之后，由传感器记录采集和接收的时间。障碍物的球面坐标利用传感器的返回时间和每次扫描后接收的功率（作为反射率）来计算。

由于激光雷达传感器的是一个特殊的球面坐标系值，让我们来复习该特殊的球面坐标系。

球面坐标系

在球面坐标系中，点由距离和两个角度定义。为了表示这两个角，我们使用方位角（Th）和极角（γ）约定。因此，点是由（r，τ，γ）定义的。

从上面的图解可以看出，方位角是在X轴上测量的X-Y平面，极坐标角是Z轴测量的Z-Y平面。
从上面的图，我们可以得到下列方程，将笛卡尔坐标转换为球面坐标。

可以使用下面的方程从球坐标导出笛卡尔坐标。

五、雷达数据处理线程
1.雷达数据处理线程

void  WRC_3D16_L0_DataProc::ProcessingLoop()
{
	int nTransformed,nReceived;
	int nBytes;
	int idx;
	unsigned char nPacket[MAX_UDP_PACKET_SIZE]; //数据包暂存

	nTransformed = 0;
	nBytes = 0;

	while (1)
	{
		pthread_mutex_lock(&g_L0_Mutex_NewPacket);//线程锁，是上面接收数据时候的线程锁
		nReceived = g_L0_ReceivedPacketNum;//上面接收雷达数据线程接收的数据包数量
		pthread_mutex_unlock(&g_L0_Mutex_NewPacket);
		//判断是否有新包,无新包时,等待
		if (nTransformed == nReceived)
		{
			usleep(PACKET_INTERVAL_TIME);
			//OUTPUT_LOG("3D:L0:NO NEW PACKET ,WAIT...............");
			//printf("3D:L0:NO NEW PACKET ,WAIT...............\n");
			continue;
		}

		pthread_mutex_lock(&g_L0_Mutex_NewPacket);
		idx = nTransformed % MAX_ONE_FRAME_PACKET;
		nBytes = g_L0_OneFramePacketSize[idx];
		memcpy(nPacket, &(g_L0_OneFrameBuf[idx][0]), nBytes);
		pthread_mutex_unlock(&g_L0_Mutex_NewPacket);
		nTransformed++;
		//对一包数据进行解码,并判断是否形成了新一帧数据
		DecodeOnePacket(nPacket);

		//判断是否满足一帧，是则存储，否则继续接收数据
		if (m_IsNewFrame)
		{
            g_L0_NavID=pDataProc_param->g_iNavID;
            g_L0_RecivedFrameNum++;
            //OUTPUT_LOG("3D:L0:g_frameID =%d",g_L0_RecivedFrameNum);
			SaveOneFrame();
			//一帧中点的数目
			//OUTPUT_LOG("m_PointNumInFrame=%d",m_PointNumInFrame);
			//OUTPUT_LOG("m_PointNumInFrame=%d",m_PointNumInFrame);
			printf("3D:L0:in frame %d,pointnum is %d:\n",g_L0_RecivedFrameNum ,m_PointNumInFrame);
			//显示障碍点
			My_OBS.OBS_Initialize();
			My_OBS.CreateOBSGrid(m_OneFramePoint,m_PointNumInFrame);

			//满足一帧后需要初始化的值
			//memset(m_DecodePoint, 0,sizeof(WRC_3D16_Point) * MAX_ONE_FRAME_POINT_NUM);
			//memset(m_OneFramePoint, 0,sizeof(WRC_3D16_Point) * MAX_ONE_FRAME_POINT_NUM);
			m_Group = 0;
			m_IsNewFrame = false;
			memset(m_UsedAngle, false, sizeof(bool) * ONE_ROUND_ANGLE_NUM);
			memset(m_Angle, 0, sizeof(int)*MAX_ONE_FRAME_GROUP_NUM);
			memset(m_IsIgnore, false, sizeof(bool)*MAX_ONE_FRAME_POINT_NUM);
			m_IsFirstAngel = true;//一个角度是否是一帧中的第一个角度
			//指向最新一包数据
			pthread_mutex_lock(&g_L0_Mutex_NewPacket);
			nTransformed = g_L0_ReceivedPacketNum;
			pthread_mutex_unlock(&g_L0_Mutex_NewPacket);

			// if (nTransformed == g_L0_ReceivedPacketNum) //判断是否有新包 无则等待
			// {
			// 	OUTPUT_LOG("no new package,just wait......");
            // 	printf("3D:no new package,wait......\n");
            // 	usleep(PACKET_INTERVAL_TIME);
            // 	continue;
        	// }
            // nTransformed = g_L0_ReceivedPacketNum;
		}
	}
}