多激光雷达与摄像头的融合算法（一）

2020年得第一篇博客，真是好长时间没发博客，这半年里毕业到一家v2x公司之后比较忙，不发博客内心是比较着急，总觉得没记录点啥。年底了，写点东西吧，不然csdn恐怕要取消我博客专家的评级了。

接下俩我分两章介绍一下文章，小编手里有两个传感器，6个激光雷达与摄像头，还有一个微波摄像头，分别装在一个路口的不同地方，需要将这些传感器进行融合输出目标列表。如图：

安装如图：

第一篇讲解如何进行通信，后面第二章讲解具体算法实现。
先摆一下最终结果吧：

激光雷达可以获取目标前方的环境信息，返回前方障碍物的距离数据，并且距离信息的精度非常高，误差可以达到厘米级别，但由于得到的是目标的几何轮廓信息，很难分辨得到运动目标与目标运动的速度（在本项目中激光雷达虽然给出目标ID与速度，但置信度是不高的）。微波雷达可以给出前方运动目标的速度，角度信息都比较准确，但距离信息不准确。摄像头在感知系统里使用频率最高，单一摄像头可完成车辆行人等二维信息获取及其细分类问题，从而弥补激光雷达的缺陷。因此本项目的主要思路是完成互补信息的投影融合。得到融合后的目标数据之后，边缘计算设备按照内部协议发送至网络中。

软件架构如下：

1、protobuf介绍
Protobuf产生于Google，是一种序列化和反序列化协议，具有空间开销小、解析速度快、兼容性好等优点，非常适合于对性能要求高的RPC（Remote Procedure Call）调用。网络上的两个程序通过一个双向的通信连接实现数据的交换，这个连接的一端称为一个Socket。本文旨在实现通过将消息使用protobuf序列化后经过Socket发送到对端，对端使用protobuf反序列化得到原始的信息。这个想法来源于在集群中从节点需要定时的发送心跳信息到主节点，以宣告自己的存在；本文旨在以自己的想法实现这种通信。
继续阅读文章之前，您需要了解protobuf的基本知识，以及unix环境中socket通信的基本知识，可以参考文章最后给出的几篇参考文档。下文代码的功能是client端会每隔5s向server端发送自己的状态信息，信息中包括client端发送此次心跳的时间和client端的地址。server端解析client端的心跳信息后输出该信息。本文的代码调试均在Linux环境下。
首先，来定义protobuf数据结构Test.proto
由于是公司项目，下面只是简单的数据结构形式，大部分变量已被略去，但不影响学习。

message Point2 {
  optional float x = 1;
  optional float y = 2;
}
message Perceptron {
  enum NS {
      N = 0; // 北纬
      S = 1;// 南纬
    };
  enum EW {
      E = 0; // 东经
      W = 1;// 西经
    };
  enum Type {
      UNKNOWN = 0; // 未知
      PEDESTRIAN = 1; // 人
      BICYCLE = 2;  // 非机动车
      CAR = 3; // 小汽车
      TRUCK_BUS = 4;  // 大车
    };
  optional string Devide_Id = 1;  // 设备号.
  optional bool  Devide_Is_True = 2;  // 判断设备数据的有效性.
  optional int32 Number_Frame = 3;  // 帧的序列号.
  optional Type Object_Class_Type = 4;  // 目标种类.
  optional int32 Object_Id = 5;  // 目标Id.
  optional Point2 Point2f  = 6;  // 目标的几何中心，单位为米.
  optional float Object_Confidence = 7;  // 目标置信度.
  optional float Object_Speed = 8; // 目标速度.
  optional float Object_acceleration = 9;  // 目标加速度.
  optional float Object_Width = 10; // 目标宽度.
  optional float Object_Length = 11; // 目标长度.
}
message PerceptronSet {
  repeated Perceptron perceptron = 1;  // Array of percepted obstacles
}

编译Test.proto生产对于的解析代码，　protoc -I=./ --python_out=./ test.proto：
其中-I是source的路径，–python_out表示对应python库的生成路径，然后是对应的proto文件。当然，pb还支持c++和java，修改–python_out即可。
完成后就有对应的test_pb2.py文件了。导入后即可使用.

接下来我给出解析与封装protobuf的实例：

发送端：

# -- coding: utf-8 --
import struct
from NL_config import *
import socket
import perceptron_pb2
def udp_sender(data_list_protobuf):
    addr = (rsu_ip, int(rsu_port))
    udp_s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    data_s = ''
    data_head = b'\xda\xdb\xdc\xdd'
    data_s += data_head.hex()
    frame_type=b'\x01'
    data_s+=frame_type.hex()
    perception_type=b'\x00'
    data_s+=perception_type.hex()
    target_number = len(data_list_protobuf)
    target_number_b = struct.pack('!h', target_number)
    data_s += target_number_b.hex()
    data_s +=data_list_protobuf.hex()
    #print(bytes().fromhex(data_s))
    udp_s.sendto( bytes().fromhex(data_s) , addr)
    udp_s.close()

#将线程传过来得全局变量字典进行序列化
def dict_protobuf(sensor_dict_data):
    perceptronList = perceptron_pb2.PerceptronSet()
    # print(sensor_dict_data)
    for obj in range(len(sensor_dict_data)):
        perceptron_objects = perceptronList.perceptron.add()
        object_data = perceptron_objects
        object_data.Devide_Id = sensor_dict_data[obj]['Devide_Id']
        object_data.Devide_Is_True = sensor_dict_data[obj]['Devide_Is_True']
        object_data.Number_Frame = sensor_dict_data[obj]['Number_Frame']
        object_data.Object_Class_Type= sensor_dict_data[obj]['Object_Class_Type']
        object_data.Object_Id = sensor_dict_data[obj]['Object_Id']
        object_data.Point2f.x = 0
        object_data.Point2f.x=0
        object_data.Object_Confidence = sensor_dict_data[obj]['Object_Confidence']
        object_data.Object_Speed = sensor_dict_data[obj]['Object_Speed']
        object_data.Object_acceleration =sensor_dict_data[obj]['Object_acceleration']
        object_data.Object_Width = sensor_dict_data[obj]['Object_Width']
        object_data.Object_Length = sensor_dict_data[obj]['Object_Length']
        object_data.Object_Height =sensor_dict_data[obj]['Object_Height']
        object_data.Object_Longitude = sensor_dict_data[obj]['Object_Longitude']
        object_data.Object_Latitude = sensor_dict_data[obj]['Object_Latitude']
        object_data.Time_stamp = sensor_dict_data[obj]['Time_stamp']
        object_data.Object_Direction = sensor_dict_data[obj]['Object_Direction']
        object_data.Object_Yaw = sensor_dict_data[obj]['Object_Yaw']
        object_data.is_tracker = sensor_dict_data[obj]['is_tracker']
        object_data.Object_Direction = sensor_dict_data[obj]['Object_Direction']
    data_perceptron = perceptronList.SerializeToString()
    return data_perceptron

解析端代码：

# -- coding: utf-8 --
import threading
from  perceptron_pb2 import Perceptron,Point2,Point3,PerceptronSet
import socket
import configparser
from  NL_config import *
import time
import numpy as np
from parse_data import parse_Sensor_data, print_data, parse_data
#激光雷达数据接收线程
class LidarReadThread(threading.Thread):
    def __init__(self,LidarSocket):
        threading.Thread.__init__(self)
        self.lidar_data=LidarSocket
        self.lidar_data.bind((lidar_ip, lidar_port))
        self.count=1
    def run(self):
        global lidar_sensor,Sensor_data
        global lidar_sensor_list 
        print("lidar thread is starting... ")
       # fw=open("./test.txt","a+")
        while True:
            msg_data,addr=self.lidar_data.recvfrom(10240)
            print("recv... ",time.time())
            target_data = PerceptronSet()
            if msg_data[:4] == b'\xda\xdb\xdc\xdd':
                #print(msg_data)
                head, FrameType, PerceptronType, DataLength, Sensor_data = parse_message(msg_data)
                #udp_sender(Sensor_data)
                t_lidar_lock.acquire()
                lidar_sensor_list.clear()
                target_data.ParseFromString(Sensor_data)
                #print("get obj ...",len(target_data.perceptron))
                for object_data in target_data.perceptron:
                    lidar_sensor = parse_Sensor_data(object_data, devide_id_name)
                    lidar_sensor_list.append(lidar_sensor)
                t_lidar_lock.release()
        msg_data.close()

其中parse_data为

# -- coding: utf-8 --
from datapoint import DataPoint,Is_working
import numpy as np

def parse_Sensor_data(target_data, devide_id_name):
    standard_data={}
    standard_data['Devide_Id']=target_data.Devide_Id
    standard_data['Devide_Is_True'] = target_data.Devide_Is_True
    standard_data['Number_Frame'] = target_data.Number_Frame
    standard_data['Object_Class_Type'] = target_data.Object_Class_Type
    if target_data.Devide_Id == devide_id_name[0]:
        standard_data['Object_Id'] = (target_data.Object_Id%10000)+10000
    if target_data.Devide_Id == devide_id_name[1]:
        standard_data['Object_Id'] = (target_data.Object_Id%20000)+20000
    if target_data.Devide_Id == devide_id_name[2]:
        standard_data['Object_Id'] = (target_data.Object_Id%30000)+30000
    if target_data.Devide_Id == devide_id_name[3]:
        standard_data['Object_Id'] = (target_data.Object_Id%40000)+40000
    standard_data['Point2f'] = target_data.Point2f
    standard_data['Object_Confidence'] = target_data.Object_Confidence
    standard_data['Object_Speed'] = target_data.Object_Speed
    standard_data['Object_acceleration'] = target_data.Object_acceleration
    standard_data['Object_Width'] = target_data.Object_Width
    standard_data['Object_Length'] = target_data.Object_Length
    standard_data['Object_Height'] = target_data.Object_Height
    standard_data['Object_Longitude'] = target_data.Object_Longitude
    standard_data['Object_Latitude'] = target_data.Object_Latitude
    standard_data['Time_stamp'] = target_data.Time_stamp
    standard_data['Object_Direction'] = target_data.Object_Direction
    standard_data['Object_Yaw'] = target_data.Object_Yaw
    standard_data['is_tracker'] = target_data.is_tracker
    standard_data['Object_Direction'] = target_data.Object_Direction
    return standard_data
def print_data(object_data):

    print("time_stamp is ", object_data.Time_stamp)
    print("Devide_Id is ", object_data.Devide_Id)
    print("Devide_Is_True is ", object_data.Devide_Is_True)
    print("Number_Frame is ", object_data.Number_Frame)
    print("Object_Class_Type is ", object_data.Object_Class_Type)
    print("Object_Id is ", object_data.Object_Id)
    print("Point2f is ", object_data.Point2f)
    print("Object_Confidence is ", object_data.Object_Confidence)
    print("Object_Speed is ", object_data.Object_Speed)
    print("Object_acceleration is ", object_data.Object_acceleration)
    print("Object_Width is ", object_data.Object_Width)
    print("Object_Length is ", object_data.Object_Length)
    print("Object_Height is ", object_data.Object_Height)
    print("Object_Longitude is ", object_data.Object_Longitude)
    print("Object_Latitude is ", object_data.Object_Latitude)
    print("Object_Direction is ", object_data.Object_Direction)
    print("Object_Yaw is ", object_data.Object_Yaw)
    print("is_tracker is ", object_data.is_tracker)


def parse_data(Source_data):
    """
    :param Sensor_data:
    字典：三个线程传过来的的传感器数据
    lidar数据，rader数据，摄像头数据
    :return:
    每个传感器数据单一属性
    """
    Sensor_data={}
    for k, v in enumerate(Source_data):
        Sensor_data.update({k : v})

    return  Sensor_data

上述主要将软件的通信思路讲解清除，这样每个传感器的目标列表就可以发送至融合算法中了。后面一篇文章我将具体介绍如何对激光雷达与摄像头的目标级融合等算法。