Signal Processing

In this post, we will discuss the perspective-n-point (PnP) problem. We will start with the problem definition. Then, gradient-based optimization methods will be introduced. Finally, we will discuss two global optimization methods. Problem Formulation The core task of the Perspective-n-Point (PnP) problem is to determine the pose—specifically, the rotation and translation—of a calibrated camera in 3D space. This is achieved by using a set of known 3D points in the world and their corresponding 2D projections observed on the camera’s image sensor. ...

In this document, we provide detailed derivations complementing those presented in [2]. Details of the Derivation Discrete Model Based on the $\boxplus$ operation defined above, we can discretize the continuous model in (1) at the IMU sampling period $\Delta t$ using a zero-order holder. The resultant discrete model is $$ \mathbf{x}_{i+1} = \mathbf{x}_i \boxplus (\Delta t f(\mathbf{x}_i, \mathbf{u}_i, \mathbf{w}_i))$$where $i$ is the index of IMU measurements, and the function $f$, state $\mathbf{x}$, input $\mathbf{u}$, and noise $\mathbf{w}$ are defined below: ...

In this post, we will discuss the Iterative Closest Point (ICP) problem: from point-to-point and point-to-plane ICP to generalized ICP. Problem Formulation Let two 3D point-sets $\mathcal{X} = \{\mathbf{x}_i\}, i = 1, \ldots, N$ and $\mathcal{Y} = \{\mathbf{y}_j\}, j = 1, \ldots, M$, where $\mathbf{x}_i, \mathbf{y}_j \in \mathbb{R}^3$ are point coordinates, be the data point-set and the model point-set respectively. The goal is to estimate a rigid motion with rotation $\mathbf{R} \in SO(3)$ and translation $\mathbf{t} \in \mathbb{R}^3$ that minimizes the following $L_2$-error $E$: ...

System Diagram See Appendix.VII Figure 1: System diagram of a typical 4D mmWave radar signal processing chain (figure from [1]) Single Object Tx-Rx Model Below is a mathematical formalization of each major step in the traditional 4D mmWave Frequency Modulated Continuous Wave (FMCW) radar signal processing chain, from transmitted signals through to point-cloud generation. Please note that these equations represent a general framework; actual implementations may vary slightly depending on specific system parameters and design choices. ...

Tracking ego pose is critical in autonomous driving. In this article, we will discuss how to fuse the IMU, wheel encoder, GPS, etc. to track the ego pose. We will derive the pose tracking algorithm based on the iterative extended Kalman filter. This document mainly follows [1] and [2]. Preliminaries Let $\mathcal{M}$ be the manifold of dimension $n$ in consideration (e.g., $\mathcal{M} = SO(3)$). Since manifolds are locally homeomorphic to $\mathbb{R}^n$, we can establish a bijective mapping from a local neighborhood on $\mathcal{M}$ to its tangent space $\mathbb{R}^n$ via two encapsulation operators $\boxplus$ and $\boxminus$: ...

When tracking an object’s position, it is typically done from the ego vehicle’s perspective. However, the ego’s motion makes tracking the object somewhat difficult. The basic idea is to perform the tracking in the world coordinate system, then transform the results into the ego-car’s coordinate system. In this post, we will discuss how to concisely incorporate the ego’s motion into the object tracking process. Continuous Form Coordinate Definitions The target’s movement in the world coordinate: $\mathbf{o}(t)$; ego-car movement in the world coordinate: $\mathbf{g}(t)$; ego-car’s heading angle: $\theta(t)$; observed target’s coordinate relative to the ego-car: $\mathbf{x}(t)$. ...

Besides object position tracking, heading angle tracking is also critical in autonomous driving. In this article, we will discuss how to track the angle of an object using the Kalman filter and how to do motion compensation. Wrap the Angle In this paper “On wrapping the Kalman filter and estimating with the SO(2) group”, the author has the conclusion: “based on the mathematically grounded framework of filtering on Lie groups, yields the same result as heuristically wrapping the angular variable within the EKF framework”. ...

Unlike global shutter cameras that capture the entire frame at once, rolling shutter cameras capture each row sequentially, leading to image distortions when there is motion. This paper discusses the rolling shutter effect in cameras and methods to handle it. Fundamentals of Rolling Shutter Camera Global Shutter Camera v.s. Rolling Shutter Camera. (a) Global Shutter Camera. (b) Rolling Shutter Camera. (figure from [6]) To efficiently capture and read the image, the time constraints for rolling shutter camera are: ...

When projecting a 3D object onto the camera plane, we usually use the pinhole model. However, it only applies to a single point. When we consider a solid object, we need to consider the interaction between the object and the camera, especially when the object is close to the camera. In the following, we will use the view frustum to cull the object and project it onto the camera plane. ...

This post discusses the assignment problem, its primal-dual interpretation, and the gated Hungarian algorithm. Tutor HungarianAlgorithm and [4] gives a nice interpretation of the dual-prime of the Hungarian algorithm. Prime-Dual Interpretation of Hungarian Algorithm The following linear program gives a lower bound on the optimal value of the assignment problem: $$\begin{array}{ll} \min & \sum_{i \in I} \sum_{j \in J} c_{i j} x_{i j} \\ \text { s.t. } & \sum_{j \in J} x_{i j}=1 \text { for all } i \in I \\ & \sum_{i \in I} x_{i j}=1 \text { for all } j \in J \\ & x_{i j} \geq 0 \end{array}$$To see this, note that we can let $x_{i j}=1$ if $i$ is assigned to $j$ and 0 otherwise. Clearly, this is a feasible solution to the L.P, so the optimal value of the LP must be at most the optimal value of the assignment problem. ...