# C++网络编程

​	本文的网络编程指代C++ 套接字程序编写，讲述常用的两种连接TCP和UDP的socket.对应**流式套接字（SOCK_STREAM）,数据报套接字(SOCK_DGRAM)**。当然还有第三种**原始套接字(SOCK_RAW)**，使用较少。

C++使用原始未封装的socket还需要考虑跨平台，windows和linux使用的库并不一样，写法也不一样，但是建立连接的方式和原理是一样的。




## TCP socket连接过程



![socket_tcp.png](/static/img/c6b438d0503dea388f338a6730def913.socket_tcp.webp)
TCP（传输控制协议）是一种面向连接的、可靠的、全双工的传输层协议。但是连接的双方并不是完全平等的，而是分为客户端和服务端。客户端和服务端的初始状态是不同的。上图比较清晰的展示了连接过程，服务端需要四步才能完成初始化进入等待连接，客户端只需要两步。 服务端需要改变socket的监听状态并通过**accept()**阻塞等待客户端的消息。

## UDP socket连接过程



![socket_udp.png](/static/img/b074152a005bb267f88edbfb30cef32f.socket_udp.webp)

UDP（用户数据报协议）是一种无连接的、不可靠的传输层协议，用于在计算机网络中进行简单的、低延迟的数据传输。同样这也是全双工，可以相互通信，但是不需要服务端不需要修改监听状态，客户端也不需要连接，只需要直接发送数据。注意：`recvfrom()` 函数是阻塞的，但是也可以使用超时和非阻塞的方式接收数据，比如可以将接收数据的操作放在一个单独的线程或进程中，使主线程或进程不被阻塞，从而实现并发处理。

## Linux socket编程

### 编程使用的相关库

使用的库【必须】：<arpa/inet.h> <sys/socket.h> <netinet/in.h>

可选的库【坑：实际是必选的close函数在里面有声明，不然无法关闭套接字】：<unistd.h>

`<arpa/inet.h>` 是 C/C++ 编程中的一个头文件，它提供了一些用于 IP 地址转换的函数。主要用于在网络编程中对 IP 地址和端口进行**网络字节序和主机字节序之间的转换**。

这个头文件提供了以下常用函数，这些函数在网络编程中常用于 IP 地址和端口的转换、显示以及配置。在使用这些函数时，请确保正确地处理返回值和错误码，以保证网络数据的正确处理。：

1. `in_addr_t inet_addr(const char *cp);`: 该函数将一个点分十进制字符串表示的 IPv4 地址（如 "192.168.1.1"）转换为 `in_addr_t` 类型的网络字节序表示。`in_addr_t` 是一个 32 位无符号整数类型，用于表示 IPv4 地址。
2. `char *inet_ntoa(struct in_addr in);`: 该函数将一个 `struct in_addr` 结构体表示的 IPv4 地址（以网络字节序存储）转换为点分十进制字符串。`in_addr` 结构体包含一个字段 `s_addr`，表示一个 IPv4 地址。
3. `int inet_pton(int af, const char *src, void *dst);`: 该函数将一个点分十进制字符串表示的 IPv4 或 IPv6 地址（取决于 `af` 参数）转换为网络字节序存储的二进制形式。`af` 参数可以是 `AF_INET`（IPv4）或 `AF_INET6`（IPv6）。
4. `const char *inet_ntop(int af, const void *src, char *dst, socklen_t size);`: 该函数将一个网络字节序存储的 IPv4 或 IPv6 地址（取决于 `af` 参数）转换为点分十进制字符串表示。`af` 参数可以是 `AF_INET`（IPv4）或 `AF_INET6`（IPv6）。



`<sys/socket.h>` 是 C/C++ 编程中的一个头文件，它提供了对套接字（socket）编程的支持。套接字是在网络编程中用于实现进程间通信的一种机制。这个头文件提供了一系列函数和常量，用于创建、绑定、监听、接受、连接、发送和接收套接字数据。通过使用 `<sys/socket.h>` 头文件提供的函数和常量，你可以在网络编程中实现不同类型的套接字通信，包括 TCP 和 UDP 等协议的通信。这个头文件是在进行网络编程时必不可少的重要工具之一。需要注意的是，使用套接字编程需要对网络协议和底层数据传输有一定的了解，同时需要仔细处理返回值和错误码以保证程序的正确性和可靠性。

一些常用的函数和常量包括：

1. **函数：**
   * `socket()`：创建一个套接字。
   * `bind()`：将套接字绑定到一个特定的 IP 地址和端口。
   * `listen()`：开始监听传入的连接请求。
   * `accept()`：接受一个传入的连接请求。
   * `connect()`：与远程套接字建立连接。
   * `send()`：发送数据。
   * `recv()`：接收数据。
2. **常量：**
   * `AF_INET`：IPv4 地址族。
   * `AF_INET6`：IPv6 地址族。
   * `SOCK_STREAM`：面向连接的套接字类型（用于 TCP）。
   * `SOCK_DGRAM`：无连接的套接字类型（用于 UDP）。

`<netinet/in.h>` 头文件通常与 `<sys/socket.h>` 和其他网络编程相关的头文件一起使用，**用于定义和处理网络地址和端口**。它是在进行网络编程时必不可少的重要工具之一。需要注意的是，这些函数主要用于处理 IPv4 地址和端口，如果要处理 IPv6 地址，需要使用 `<netinet/in6.h>` 头文件和相应的数据结构和函数。

主要功能包括：

1. **数据结构定义：**
   * `struct sockaddr_in`：用于表示 IPv4 地址和端口的数据结构。
   * `struct in_addr`：用于表示 IPv4 地址的数据结构。
2. **函数：**
   * `htons()` 和 `ntohs()`：用于主机字节序和网络字节序之间的 16 位整数转换。
   * `htonl()` 和 `ntohl()`：用于主机字节序和网络字节序之间的 32 位整数转换。
   * `inet_addr()`：将点分十进制表示的 IPv4 地址转换为网络字节序的二进制表示。
   * `inet_ntoa()`：将网络字节序的二进制表示的 IPv4 地址转换为点分十进制表示的字符串。



`<unistd.h>` 提供了一组与 POSIX（可移植操作系统接口）标准相关的系统调用（system call）函数和符号常量。这些函数和常量主要用于在操作系统层面进行系统调用和对操作系统提供的一些基本功能进行访问。【设计并行处理的时候可能需要】

一些常用的函数和常量包括：

1. **函数：**
   * `read()`：从文件描述符中读取数据。
   * `write()`：向文件描述符写入数据。
   * `close()`：关闭文件描述符。
   * `fork()`：创建一个新进程（通过复制当前进程）。
   * `exec()`：用于在当前进程中执行新程序。
   * `pipe()`：创建一个管道，用于进程间通信。
   * `getpid()`：获取当前进程的进程ID。
   * `getuid()`：获取当前用户的用户ID。
   * `sleep()`：让当前进程睡眠一段时间。
   * `usleep()`：让当前进程以微秒级别睡眠。
   * `access()`：检查文件或目录的访问权限。
2. **常量：**
   * `STDIN_FILENO`：标准输入文件描述符。
   * `STDOUT_FILENO`：标准输出文件描述符。
   * `STDERR_FILENO`：标准错误输出文件描述符。

### TCP代码详解

服务端完整代码：

```cpp
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <cerrno>
#include <cstring>

#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <unistd.h>
#include <arpa/inet.h>

int main() {
    std::cout << "This is server" << std::endl;
    // socket
    int listenfd = socket(AF_INET, SOCK_STREAM, 0);
    if (listenfd == -1) {
        std::cout << "Error: socket" << std::endl;
        return 0;
    }
    // bind
    struct sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(8000);
    addr.sin_addr.s_addr = INADDR_ANY;
    if (bind(listenfd, (struct sockaddr*)&addr, sizeof(addr)) == -1) {
        std::cout << "Error: bind" << std::endl;
        return 0;
    }
    // listen
    if(listen(listenfd, 5) == -1) {
        std::cout << "Error: listen" << std::endl;
        return 0;
    }
    // accept
    int conn;
    char clientIP[INET_ADDRSTRLEN] = "";
    struct sockaddr_in clientAddr;
    socklen_t clientAddrLen = sizeof(clientAddr);
    while (true) {
        std::cout << "...listening" << std::endl;
        conn = accept(listenfd, (struct sockaddr*)&clientAddr, &clientAddrLen);
        if (conn < 0) {
            std::cout << "Error: accept" << std::endl;
            continue;
        }
        inet_ntop(AF_INET, &clientAddr.sin_addr, clientIP, INET_ADDRSTRLEN);
        std::cout << "...connect " << clientIP << ":" << ntohs(clientAddr.sin_port) << std::endl;

        char buf[255];
        while (true) {
            /// 此处数据处理会阻塞主线程，可以使用多线程处理数据，主线程继续accept其它的客户端连接。
            memset(buf, 0, sizeof(buf));
            int len = recv(conn, buf, sizeof(buf), 0);
            buf[len] = '\0';
            if (strcmp(buf, "exit") == 0) {
                std::cout << "...disconnect " << clientIP << ":" << ntohs(clientAddr.sin_port) << std::endl;
                break;
            }
            std::cout << buf << std::endl;
            send(conn, buf, len, 0);
        }
        
        close(conn);
    }
    close(listenfd);
    return 0;
}
```

下文将会截取以上代码的核心片段进行解析，如下：
```cpp
int socket(int domain, int type, int protocol);
```

创建一个socket返回唯一一个表示符

**domain**：指定协议域，或称协议族（family），决定了socket的地址类型。

| 名称             | 含义              | 名称         | 含义                     |
| ---------------- | ----------------- | ------------ | ------------------------ |
| PF_UNIX,PF_LOCAL | 本地通信          | PF_X25       | ITU-T X25 / ISO-8208协议 |
| AF_INET,PF_INET  | IPv4 Internet协议 | PF_AX25      | Amateur radio AX.25      |
| PF_INET6         | IPv6 Internet协议 | PF_ATMPVC    | 原始ATM PVC访问          |
| PF_IPX           | IPX-Novell协议    | PF_APPLETALK | Appletalk                |
| PF_NETLINK       | 内核用户界面设备  | PF_PACKET    | 底层包访问               |

**type**：指定socket类型。

| 名称           | 含义                                                         |
| -------------- | ------------------------------------------------------------ |
| SOCK_STREAM    | Tcp连接，提供序列化的、可靠的、双向连接的字节流。支持带外数据传输 |
| SOCK_DGRAM     | 支持UDP连接（无连接状态的消息）                              |
| SOCK_SEQPACKET | 序列化包，提供一个序列化的、可靠的、双向的基本连接的数据传输通道，数据长度定常。每次调用读系统调用时数据需要将全部数据读出 |
| SOCK_RAW       | RAW类型，提供原始网络协议访问                                |
| SOCK_RDM       | 提供可靠的数据报文，不过可能数据会有乱序                     |
| SOCK_PACKET    | 这是一个专用类型，不能呢过在通用程序中使用                   |

**protocol**：指定协议。常用的协议有，IPPROTO_TCP、IPPTOTO_UDP、IPPROTO_SCTP、IPPROTO_TIPC等，它们分别对应TCP传输协议、UDP传输协议、STCP传输协议、TIPC传输协议。这个参数一般填入0，会自动选择。

```cpp
int bind(int sockfd, const struct sockaddr *addr, socklen_t addrlen);
```

**sockfd**：即要操作的socket描述字，socket()函数的返回值。
**addr**：一个const struct sockaddr *指针，指向要绑定给sockfd的协议地址。这个地址结构根据地址创建socket时的地址协议族的不同而不同。此处一般将 struct sockaddr_in addr的指针 转换成sockaddr类型的指针。

源码中的结构体定义：

```cpp
// ipv4
struct sockaddr_in {
    sa_family_t    sin_family; /* address family: AF_INET */
    in_port_t      sin_port;   /* port in network byte order */
    struct in_addr sin_addr;   /* internet address */
};
转换后的类型：
/* Structure describing a generic socket address.  */
struct sockaddr
  {
    __SOCKADDR_COMMON (sa_);	/* Common data: address family and length.  */
    char sa_data[14];		/* Address data.  */
  };

/* Internet address. */
struct in_addr {
    uint32_t       s_addr;     /* address in network byte order */
};

// ipv6
struct sockaddr_in6 { 
    sa_family_t     sin6_family;   /* AF_INET6 */ 
    in_port_t       sin6_port;     /* port number */ 
    uint32_t        sin6_flowinfo; /* IPv6 flow information */ 
    struct in6_addr sin6_addr;     /* IPv6 address */ 
    uint32_t        sin6_scope_id; /* Scope ID (new in 2.4) */ 
};
struct in6_addr { 
    unsigned char   s6_addr[16];   /* IPv6 address */ 
};

// Unix
#define UNIX_PATH_MAX    108
struct sockaddr_un { 
    sa_family_t sun_family;               /* AF_UNIX */ 
    char        sun_path[UNIX_PATH_MAX];  /* pathname */ 
};
```

```cpp
int listen(int sockfd, int backlog);
```

**sockfd**：即为要监听的socket描述字，
**blacklog**：为相应socket可以排队的最大连接个数。
socket()函数创建的socket默认是一个主动类型的，listen函数将socket变为被动类型的，等待客户的连接请求。

```cpp
int accept(int sockfd, struct sockaddr *addr, socklen_t *addrlen);
```

**sockfd**：为服务器的socket描述字，
**addr**：为指向struct sockaddr *的指针，用于返回客户端的协议地址，
**addrlen**：为协议地址的长度。

**返回值**：由内核自动生成的一个全新的描述字，代表与返回客户的TCP连接。

注意：accept的第一个参数为服务器的socket描述字，是服务器开始调用socket()函数生成的，称为监听socket描述字；而accept函数返回的是已连接的socket描述字。一个服务器通常通常仅仅只创建一个监听socket描述字，它在该服务器的生命周期内一直存在。内核为每个由服务器进程接受的客户连接创建了一个已连接socket描述字，当服务器完成了对某个客户的服务，相应的已连接socket描述字就被关闭。

```cpp
send(conn, buf, len, 0);
```

代码解释如下源码的注释：

```cpp
/* Send N bytes of BUF to socket FD.  Returns the number sent or -1.

  This function is a cancellation point and therefore not marked with

  __THROW.  */

extern ssize_t send (int __fd, const void *__buf, size_t __n, int __flags);

/* Read N bytes into BUF from socket FD.

  Returns the number read or -1 for errors.

  This function is a cancellation point and therefore not marked with

  __THROW.  */

extern ssize_t recv (int __fd, void *__buf, size_t __n, int __flags);
```

###  UDP代码详解

完整的服务端代码：

```cpp
#include <iostream>
#include <cstring>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h> 

int main() {
    int sockfd;
    struct sockaddr_in serverAddr, clientAddr;
    char buffer[1024];

    // Create UDP socket
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd < 0) {
        std::cerr << "Error creating socket." << std::endl;
        return 1;
    }

    // Set up server address
    memset(&serverAddr, 0, sizeof(serverAddr));
    serverAddr.sin_family = AF_INET;
    serverAddr.sin_addr.s_addr = INADDR_ANY; // Accept connections from any IP
    serverAddr.sin_port = htons(12345);      // Port to bind to

    // Bind socket to server address
    if (bind(sockfd, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) {
        std::cerr << "Error binding socket." << std::endl;
        close(sockfd); 
        return 1;
    }

    std::cout << "UDP server is running and waiting for data..." << std::endl;

    while (true) {
        socklen_t clientAddrLen = sizeof(clientAddr);
        // Receive data from client
        ssize_t receivedBytes = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0,(struct sockaddr*)&clientAddr, &clientAddrLen);
        if (receivedBytes > 0) {
            buffer[receivedBytes] = '\0';
            std::cout << "Received data from client: " << buffer << std::endl;

            // Send response back to client
            // 也可以不发送，不响应
            sendto(sockfd, buffer, receivedBytes, 0, (struct sockaddr*)&clientAddr, clientAddrLen);
        }
    }

    close(sockfd); // Close the socket before exiting the program
    return 0;
}

```

相比较TCP的服务端，UDP服务端同样需要创建套接字，同样绑定端口。不同在于不需要listen和accept函数的用。直接使用函数recvfrom阻塞当前主线程用于等待接收数据。

差异代码：

```cpp
 ssize_t receivedBytes = recvfrom(sockfd, buffer, sizeof(buffer) , 0,(struct sockaddr*)&clientAddr, &clientAddrLen);

// 源码声明
__fortify_function ssize_t
recvfrom (int __fd, void *__restrict __buf, size_t __n, int __flags,
	  __SOCKADDR_ARG __addr, socklen_t *__restrict __addr_len)
```

**sockfd**: 用于接收数据的socket标识符

**buffer**: 缓冲区

**n**:缓冲区大小

**flag**:默认填0

**(struct sockaddr*)&clientAddr**: 用于存储客户端sokcet信息的地址（指针）

**&clientAddrLen**： 地址长度

返回值：实际接收的数据量，字节数

udp客户端完整代码：



```cpp

#include <iostream>
#include <cstring>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h> 

int main() {
    int sockfd;
    struct sockaddr_in serverAddr;
    char buffer[1024];

// Create UDP socket
sockfd = socket(AF_INET, SOCK_DGRAM, 0);
if (sockfd < 0) {
    std::cerr << "Error creating socket." << std::endl;
    return 1;
}

// Set up server address
memset(&serverAddr, 0, sizeof(serverAddr));
serverAddr.sin_family = AF_INET;
serverAddr.sin_port = htons(12345);  // Server port
inet_pton(AF_INET, "127.0.0.1", &serverAddr.sin_addr); // Server IP address

// Send a message to the server
const char* message = "Hello, UDP Server!";
sendto(sockfd, message, strlen(message), 0, (struct sockaddr*)&serverAddr, sizeof(serverAddr));

// Wait for the server's response
struct sockaddr_in responseAddr;
socklen_t responseAddrLen = sizeof(responseAddr);
ssize_t receivedBytes = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0,
                                 (struct sockaddr*)&responseAddr, &responseAddrLen);
if (receivedBytes > 0) {
    buffer[receivedBytes] = '\0';
    std::cout << "Received response from server: " << buffer << std::endl;
} else {
    std::cerr << "Error receiving response from server." << std::endl;
}

close(sockfd); // Close the socket
return 0;
}
```
## Windows socket编程

--待更新

C++ windows网络编程

# 常见雷达类型


1.根据雷达工作频段的不同[**注：雷达工作频率越高，波长越短，雷达的分辨率也会随之提高,探测距离减少：随着雷达工作频率的增加，雷达波束散射和吸收的效应会增加，这会使雷达探测距离减少，对目标反射率敏感：高频信号对目标的反射率更加敏感**]，可以将雷达分为以下几类：




- X波段雷达：工作频段为8-12 GHz，主要用于民航雷达、海上航行雷达等。
- S波段雷达：工作频段为2-4 GHz，主要用于天气雷达、飞机高度测量雷达等。
- C波段雷达：工作频段为4-8 GHz，主要用于空管雷达、舰船雷达等。
- Ku波段雷达：工作频段为12-18 GHz，主要用于卫星雷达、舰船雷达等。
- Ka波段雷达：工作频段为26-40 GHz，主要用于气象雷达、远距离探测雷达等。
- W波段雷达：工作频段为75-110 GHz，主要用于高分辨率雷达成像等。

2.根据雷达的应用领域和功能，可以将雷达分为以下几类：

- 天气雷达：用于气象探测和预报。
- 监视雷达：用于监视目标的位置、速度和方向等信息，如航空雷达、海洋雷达等。
- 战术雷达：用于战争和军事目的，如防空雷达、雷达导弹等。
- **民用雷达：用于民用领域，如车载雷达、安检雷达等。**

3.根据雷达的工作方式，可以将雷达分为以下几类：

- 连续波雷达：发射和接收信号的频率相同，无法测量距离，主要用于速度测量和距离预警。
- 脉冲雷达：通过发射短脉冲信号，接收回波来测量目标距离、速度等信息，主要用于空管雷达、气象雷达等。
- 相控阵雷达：通过调整多个天线的相位和幅度，实现波束的控制和调制，具有高分辨率和目标跟踪能力，主要用于防空雷达、舰船雷达等。

## 自动驾驶使用的雷达

自动驾驶中常用的激光雷达有多种，其中较为常见的是旋转式激光雷达（Rotating LiDAR）和固态激光雷达（Solid-State LiDAR），旋转式激光雷达是一种通过旋转激光发射器和接收器组成的激光雷达，可以360度无死角地扫描周围环境。旋转式激光雷达具有测量精度高、测量距离远等优点，但由于需要旋转机械部件，其体积较大、成本较高。

固态激光雷达则是一种采用固态激光器阵列进行扫描的激光雷达，其体积较小、功耗较低，能够适应更多场景的需求。固态激光雷达目前也逐渐被自动驾驶系统所采用，并在一些场景中取代了传统的旋转式激光雷达。

## 海上小目标雷达

在海上测量小目标，需要选择具有较高分辨率和探测灵敏度的雷达，常用的选择包括以下几种：

1. X波段雷达：X波段雷达工作频率在8-12 GHz之间，具有较高的分辨率和较小的波长，可以用于探测小目标，例如小型船只和浮标等。X波段雷达在海上应用广泛，可以实现较高精度的目标探测和跟踪。
2. Ku波段雷达：Ku波段雷达工作频率在12-18 GHz之间，与X波段雷达相比，具有更高的分辨率和较小的波长。Ku波段雷达通常用于探测小型船只、浮标、救生筏等小目标，可以实现高精度的目标识别和跟踪。
3. S波段雷达：S波段雷达工作频率在2-4 GHz之间，波长较长，但具有较高的穿透能力和较大的测量范围。S波段雷达通常用于探测大型船只和岸边建筑物等目标，但也可以用于探测小目标。

常见雷达类型

# 激光数据处理

主要流程：数据读取->数据保存->数据预处理->数据输入模型->输出结果




## 数据格式

二进制格式: bin ,详细参考KITTI数据集的解析代码

数据包:pcap 

通用格式: pcd格式 (x,y,z,i), i表示强度值，不同物体反射强度值不一样，可以根据强度值绘制灰度图

## 相关工具

point cloud library, rviz,ros

## 预处理

1. 解析raw data,详细参考本文件夹的激光雷达手册
2. 运动畸变补偿


![33db5ec1c4d80b8ab93594d422afc336.png](/static/img/7f1cf5aa113631dee6cce8fbe98e41ae.33db5ec1c4d80b8ab93594d422afc336.webp)

### 纯估计方法(ICP/VICP)

迭代最近点（ICP）方法是采用ICP算法来匹配两个点云，通过不断地算法迭代后，将点云之间的误差缩至最小。VICP方法是ICP算法的变种形式，模型假设车辆是在匀速运动，在进行匹配点云的同时估计车辆的自身速度。迭代最近点（ICP）和VICP被统称为“纯估计方法”。

### 传感器辅助方法(IMU/ODOM)

惯性测量单元（IMU）方法是在IMU队列中查找相邻两帧IMU的数据，然后通过球面线性插值的方式计算扫描点所在时刻的激光雷达位姿，并应用齐次坐标系变化将两个点云坐标变换至同一坐标系下。轮式里程计（ODOM）方法是通过求解当前帧激光雷达数据中每个点云对应的坐标系下的里程计位姿后，再根据求得的位姿把每个点云坐标都转化到同一坐标系下（需要转化两次），最后重新封装该帧点云数据。惯性测量单元（IMU）和轮式里程计（ODOM）被统称为传感器辅助方法。

### 融合的方法

该方法是同时使用里程计和ICP的融合方案，会先利用里程计方法进行矫正，去除大部分的运动畸变，再通过ICP方法进行匹配，得到里程计的误差值，再把误差值均摊到每个点云上，并重新对点云位置进行修正。最后，再利用ICP方法进行迭代，直至误差收敛为止。


### 点云组帧

单个点云数据包的点云较少，通常会把多个数据包叠加到同一帧上，让这一帧上的点云数据能包含上万个点云，以便后续感知和定位流程的处理。

### 外参变化

点云数据通过解析得到的点云坐标系属于激光雷达坐标系，而在实际的自动驾驶技术应用中，仍需要将激光雷达的坐标系转化为车辆的坐标系，这个建立联系的过程称之为点云的外参变化。

由于激光雷达与车体是刚性连接，所以在车辆运动过程中，两者之间的相对姿态和位移是固定不变的，只需要建立两者相对坐标系之间的位置关系，通过旋转或者平移的方式，就能将这两个三维坐标系统一到一个三维坐标系下（也称为全局坐标系或世界坐标系）。

### 滤波处理

受各种因素影响，点云数据中通常会带有噪点（对模型处理无用的点）和离群点（远离主观测区域的点）。通常采用算法过滤，常用算法有：平均值滤波，中位值滤波，一阶滤波，卡尔曼滤波。



## 感知模型和算法

传统算法：

1. 地面点云分割: **平面栅格法**，**点云法向量**，**模型拟合法-平面拟合（RANSAC）**，**面元网格法**，
2. 目标物的点云分割: **基于边缘的方法**, **基于区域增长的方法**,**基于属性的方法**
3. 目标聚类分析: **K-means**,**DBSCAN**,**欧式聚类**
4. 匹配与跟踪: 配和跟踪的算法流程是先将目标预测的结果与测量的点云数据计算关联矩阵，然后利用匈牙利算法（其核心原理是寻找增广路径，从而达成最大匹配）进行匹配关系的确定，最后将点云数据分为匹配上的目标和未匹配上的目标两类，将其分别保存，并为跟踪做准备。

基于深度学习：

- **PointNet**，**PointNet++**，**VoxelNet**，**SECOND**，**PointPillar**，**PCT**

## 地图匹配

在提取完周围目标物的特征后，感知算法人员就需要根据这些特征来进行点云地图匹配，来获取各个点云之间的相对位姿。点云地图匹配一般可分为帧间匹配和高精地图匹配。

- 帧间匹配，也叫子图匹配，指将前后帧上有相同特征的点云做匹配，最后得到一张局部小地图。

- 高精地图匹配，指将优化后的点云与高精地图做匹配。

3D激光点云处理流程

# KITTI数据集报告

​	KITTI数据集是自动驾驶数据当中最出名的数据集，其采集的数据主要包含激光雷达、相机、GPS和IMU的数据。相机数据包含两个灰度相机，两个彩色相机。在研究中使用的是经同步与校正处理的双目彩色图像序列，以png格式存储，大小1240*370左右；




​	对于相机和激光雷达其中最重要的就是数据的标注和校准，对于相机内的标注对象，适用3Dbox的方式提供注释，用激光雷达的坐标表示。每一个标注对象被赋予一个类别：行人、车辆，自行车等，还有对应的3D尺寸：长、宽、高。对于标注的角度，只提供了标注对象的偏航角：标注对象的X轴相对于激光雷达X轴的角度。另外标注信息还包括了遮挡和阶段的程度，用0-1的浮点数表示。

​	对于传感器的标定，首先最重要的就是坐标系统的定义。

* ​	相机坐标系统：x表示右方，y表示下方，z表示前方。 
* ​	雷达坐标系统：x表示前方，y表示左方，z表示上方。

传感器安装位置：



![v2-a1bb3c629446d74689c4331a53282229_720w.webp](/static/img/5bb513c0a51ffb696aab6b37e6a1fdf6.v2-a1bb3c629446d74689c4331a53282229_720w.webp)


## 数据校准

一般以0号相加为参考，将3D点投影到第i个相机画面的像素中去：

$ y = P_{rect}^{(i)} R_{rect}^{(i)}x$

此处的x应该是相机的坐标系下的3D点的位置，要得到x还需要从激光雷达的坐标系转换到相机的坐标系，乘以齐次转换矩阵,所以完整的公式是：

$y = P_{rect}^{(i)} R_{rect}^{(i)}T_{velo}^{cam}x$

注：此处的x是雷达参考系的3D点云向量


![v2-2eb109ce92dd4992981cad10775104aa_720w.webp](/static/img/0c00169a9347a67b74548fe6d17b45f1.v2-2eb109ce92dd4992981cad10775104aa_720w.webp)

## 补充知识

​	对于KITTI数据集涉及的校准有：

1. 相机内参校准：包括相机的焦距、主点位置、畸变等参数的校准，以确保相机捕捉到的图像和真实世界尺度之间的正确匹配。
2. 相机和激光雷达之间的外参校准：确定相机和激光雷达之间的相对位置和姿态，以便在3D空间中将它们的数据进行配准。
3. 激光雷达的时间戳校准：激光雷达和相机的数据可能是在不同的时间采集的，因此需要对它们进行时间戳的校准，以确保它们的数据可以在时间轴上正确对齐。
4. IMU校准：如果存在惯性测量单元（IMU），则需要校准IMU的陀螺仪和加速度计数据，以减少因姿态估计和加速度计偏差等原因引起的误差。
5. 车辆姿态校准：车辆的姿态（如俯仰角、偏航角、横滚角等）也需要进行校准，以确保车辆的运动轨迹可以准确地捕捉到。

  立体相机可以通过计算两个相机之间的视差（Disparity）来获取深度信息。视差是指在两个相机的图像中，同一点在图像中的位置差异，即两个相机中的对应点之间的水平像素偏移量。

在实际应用中，视差通常由以下步骤计算得到：

1. 对两幅图像进行预处理，例如去噪、对齐等。
2. 从左图像中选取一个像素点 P，通过搜索在右图像中对应的像素点 P'，找到 P 和 P' 之间的视差。
3. 在搜索 P' 的过程中，可以使用多种算法来加速计算，例如区域匹配、特征匹配等。其中最常用的是区域匹配算法，如半局部匹配（Semi-Global Matching，SGM）算法和快速可扩展的立体视觉（Fast and Scalable Stereo，FASSTER）算法。
4. 通过相机标定得到的基线距离和两个相机的内参，可以将视差转换为深度信息。这个转换公式为：深度 = 基线距离 × 左相机焦距 / 视差。

相机内参校准是计算机视觉中的一项重要任务，旨在确定相机的内部参数，以便从图像像素坐标到真实世界尺度之间进行正确的转换。相机内参校准通常需要计算以下参数：

1. 焦距（Focal length）：焦距是指相机的成像镜头到成像平面之间的距离，通常以毫米（mm）为单位。焦距越大，表示相机能够捕捉到更远的目标，但也会导致图像失真。
2. 主点位置（Principal point）：主点是指相机成像平面上的一个点，通常位于图像的中心位置。主点的位置可以影响图像的畸变和校正结果。
3. 畸变参数（Distortion parameters）：畸变是由于相机镜头成像时产生的一种误差，导致图像中的直线不是直的。畸变通常可以分为径向畸变和切向畸变两种类型。

相机内参校准的目的是确定这些参数的值。一般情况下，可以通过拍摄标定板（如棋盘格）并测量板上点的位置，从而推导出相机内部参数的值。具体步骤如下：

1. 拍摄标定板：将标定板放在相机的视野内，并拍摄多张照片，以获得不同的视角和距离。
2. 检测标定板：使用计算机视觉技术来检测标定板上的角点，以便将其与真实世界中的位置进行匹配。
3. 计算相机内参：使用检测到的标定板角点的像素坐标和真实世界中的位置，可以通过最小二乘法等方法计算出相机的内部参数，如焦距、主点位置和畸变参数等。
4. 评估校准效果：通过将标定板拍摄的图像与真实世界中的位置进行比较，可以评估相机内参校准的效果，并进行调整。

  在计算机视觉和机器人领域中，我们经常使用刚体变换（Rigid Transform）来描述一个刚体在空间中的运动或变形。这个变换包括旋转和平移，可以用一个矩阵来表示。这样，我们可以通过矩阵运算来描述一个刚体在运动或变形过程中的姿态和位置。

## 专业名词

panoramic image：全景图

disparity: 视差

KITTI数据集详解

# 卡尔曼滤波建模

在目标追踪领域中，最常用的状态估计算法就是卡尔曼滤波，当目标短暂被遮挡的时候，为了估测遮挡期间目标的状态以及遮挡之后避免ID SWITCH的问题，需要利用卡尔曼滤波去估测短暂丢失的目标，本文介绍一种在图像目标追踪中的卡尔曼滤波模型。



## 状态转移建模

状态方程：$x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}$

系统状态 $X = [x,y,w,h,dx,dy]$

状态转移矩阵 $A = \begin{pmatrix} 1 & 0 & 0 & 0 & 1 & 0 \\ 0 & 1 & 0 & 0 & 0 & 1 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{pmatrix}$

x: box中心y坐标

y: box中心y坐标

w: box宽

h: box 高

dx: 中心点x坐标变化量 (速度)  $x_{t} - x_{t-1}$

dy：中心点y坐标变化量(速度) $y_{t} - y_{t-1}$

$AX$的结果，即$x_{t+1} = x_{t-1} + dx_{t-1}$

B: 控制输入矩阵 ， 直接为零矩阵，没有控制外力

$w_{k-1}$: 过程噪声，不可测量，直接认为成高斯分布的噪声 分布概率为 $p(w) - N(0,Q)$, $u = 0, \sigma^{2} = Q $表示方差，*Q* 表示高斯分布的方差，它控制了分布的宽度。如果 *Q* 较小，分布将非常窄，表明数据点紧密地围绕均值0分布；如果 *Q* 较大，分布将较宽，表明数据点在均值0周围有较大的离散性。*μ*=0，即分布的中心位于原点

## 观测建模

观测方程 : $z_k = Hx_{k} + v_k$

t时刻的最优估计值与t+1时刻所有框，IOU最大的为t+1时刻的观测值

状态观测矩阵:$H = \begin{pmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{pmatrix}$

$v_k$：表示观测过程中的噪声 ， $p(v) - N(0,R)$

卡尔曼滤波在图像目标追踪的应用

Welcome to my tech blog! Here, I document my coding journey and explore various technologies, focusing on practical solutions and streamlined techniques. From programming insights to tool recommendations, I share hands-on experiences that help sharpen your skills. Let’s learn and grow together!