Bundle adjustment, explained: from reprojection error to GPU solvers

The problem: many cameras, many points, one costBài toán: nhiều camera, nhiều điểm, một hàm chi phí

Bundle adjustment (BA) is the last and most accurate step of most 3D reconstruction and visual SLAM systems. It starts from rough camera poses and 3D points, for example from triangulation or a SLAM front-end, and adjusts all of them together until every point projects where it was observed. The name comes from photogrammetry: each camera sends out a bundle of light rays, and BA adjusts the bundles until the rays that see the same point meet in it.

Tối ưu bó (bundle adjustment, BA) là bước cuối cùng và chính xác nhất của phần lớn hệ tái tạo 3D và SLAM thị giác. Nó xuất phát từ các tư thế camera và điểm 3D còn thô, ví dụ có được từ phép tam giác hóa hoặc từ front-end của SLAM, rồi điều chỉnh tất cả cùng lúc cho đến khi mỗi điểm chiếu đúng vào nơi nó được quan sát. Tên gọi xuất phát từ ngành trắc địa ảnh: mỗi camera phát ra một bó tia sáng, và BA điều chỉnh các bó tia cho đến khi những tia cùng nhìn thấy một điểm gặp nhau tại điểm đó.

Figure 1. Camera i measures point j at pixel $\mathbf{u}_{ij}$ (filled dot, dashed ray). The current estimates $\mathbf{T}_i$ and $\mathbf{X}_j$ predict the pixel $\pi(\mathbf{T}_i\mathbf{X}_j)$ (circle, solid ray). Their difference $\mathbf{r}_{ij}$ is the reprojection error. BA moves all cameras and points until each predicted pixel lands on its measurement and the rays of each point meet.Hình 1. Camera i đo điểm j tại điểm ảnh $\mathbf{u}_{ij}$ (chấm đặc, tia nét đứt). Các ước lượng hiện tại $\mathbf{T}_i$ và $\mathbf{X}_j$ dự đoán điểm ảnh $\pi(\mathbf{T}_i\mathbf{X}_j)$ (vòng tròn, tia nét liền). Hiệu của chúng, $\mathbf{r}_{ij}$, là sai số chiếu lại. BA dịch chuyển mọi camera và điểm cho đến khi mỗi điểm ảnh dự đoán trùng với phép đo của nó và các tia của mỗi điểm gặp nhau.

Camera i has the pose $\mathbf{T}_i = (\mathbf{R}_i, \mathbf{t}_i)$, which maps world points into the camera frame. A point $\mathbf{X}_j$ lands at $\mathbf{P}_{ij} = (x, y, z)$ in the camera, and the pinhole model, with focal lengths $f_x, f_y$ and principal point $c_x, c_y$, projects it to a pixel:

Camera i có tư thế $\mathbf{T}_i = (\mathbf{R}_i, \mathbf{t}_i)$, biến điểm trong hệ tọa độ thế giới sang hệ tọa độ camera. Điểm $\mathbf{X}_j$ nằm tại $\mathbf{P}_{ij} = (x, y, z)$ trong camera, và mô hình lỗ kim, với tiêu cự $f_x, f_y$ và tâm ảnh $c_x, c_y$, chiếu nó thành một điểm ảnh:

$$\mathbf{P}_{ij} = \mathbf{R}_i\mathbf{X}_j + \mathbf{t}_i = \begin{bmatrix}x\\ y\\ z\end{bmatrix}, \qquad \pi(\mathbf{P}) = \begin{bmatrix} f_x\,x/z + c_x\\ f_y\,y/z + c_y\end{bmatrix}$$

The reprojection error is the predicted minus the measured pixel. BA minimizes the sum of the squared errors over all observations $\mathcal{O}$. Each squared error $e_{ij}$ is weighted by the measurement covariance $\boldsymbol{\Sigma}_{ij}$ (often $\sigma^2\mathbf{I}$ with $\sigma$ about one pixel) and passed through a robust kernel $\rho$ that limits the influence of wrong matches (see below; $\rho(e) = e$ is plain least squares):

Sai số chiếu lại là điểm ảnh dự đoán trừ điểm ảnh đo được. BA tối thiểu tổng bình phương sai số trên mọi quan sát $\mathcal{O}$. Mỗi bình phương sai số $e_{ij}$ được lấy trọng số theo hiệp phương sai của phép đo $\boldsymbol{\Sigma}_{ij}$ (thường là $\sigma^2\mathbf{I}$ với $\sigma$ khoảng một điểm ảnh) rồi đi qua một hàm bền vững $\rho$ giúp hạn chế ảnh hưởng của các cặp khớp sai (xem bên dưới; $\rho(e) = e$ là bình phương tối thiểu thông thường):

$$\mathbf{r}_{ij} = \pi\big(\mathbf{T}_i\mathbf{X}_j\big) - \mathbf{u}_{ij} \in \mathbb{R}^2, \qquad e_{ij} = \mathbf{r}_{ij}^\top\boldsymbol{\Sigma}_{ij}^{-1}\mathbf{r}_{ij}$$ $$\min_{\{\mathbf{T}_i\},\,\{\mathbf{X}_j\}}\; F = \frac{1}{2}\sum_{(i,j)\in\mathcal{O}} \rho\big(e_{ij}\big)$$

With m cameras and n points there are $6m + 3n$ unknowns, more if the intrinsics and the lens distortion are refined too. This is a nonlinear least-squares problem that is large but very sparse: each error depends on one camera and one point only. The rest of the post is about how to use that.

Với m camera và n điểm, bài toán có $6m + 3n$ ẩn, nhiều hơn nếu tinh chỉnh cả thông số nội và méo ống kính. Đây là bài toán bình phương tối thiểu phi tuyến lớn nhưng rất thưa: mỗi sai số chỉ phụ thuộc vào một camera và một điểm. Phần còn lại của bài viết nói về cách tận dụng điều đó.

Gauss–Newton and Levenberg–MarquardtGauss–Newton và Levenberg–Marquardt

Stack all unknowns in $\mathbf{x}$ and all errors in $\mathbf{r}(\mathbf{x})$. BA iterates: linearize the errors around the current estimate, solve for a step $\boldsymbol{\delta}$, apply it, repeat. With the Jacobian $\mathbf{J} = \partial\mathbf{r}/\partial\boldsymbol{\delta}$ and the weights $\mathbf{W}$ (the $\boldsymbol{\Sigma}^{-1}$ blocks times the robust weights), minimizing the linearized cost gives the Gauss–Newton normal equations:

Gom mọi ẩn vào $\mathbf{x}$ và mọi sai số vào $\mathbf{r}(\mathbf{x})$. BA lặp lại: tuyến tính hóa sai số quanh ước lượng hiện tại, giải tìm bước $\boldsymbol{\delta}$, cập nhật, rồi lặp lại. Với ma trận Jacobian $\mathbf{J} = \partial\mathbf{r}/\partial\boldsymbol{\delta}$ và trọng số $\mathbf{W}$ (các khối $\boldsymbol{\Sigma}^{-1}$ nhân với trọng số bền vững), tối thiểu hàm chi phí đã tuyến tính hóa cho ta hệ phương trình chuẩn Gauss–Newton:

$$\mathbf{r}(\mathbf{x}\boxplus\boldsymbol{\delta}) \approx \mathbf{r}(\mathbf{x}) + \mathbf{J}\boldsymbol{\delta} \quad\Longrightarrow\quad \underbrace{\mathbf{J}^\top\mathbf{W}\mathbf{J}}_{\mathbf{H}}\;\boldsymbol{\delta} = -\underbrace{\mathbf{J}^\top\mathbf{W}\mathbf{r}}_{\mathbf{g}}$$

$\mathbf{H}$ approximates the Hessian of F without the second derivatives of $\mathbf{r}$. Far from the solution the Gauss–Newton step can overshoot, so Levenberg–Marquardt (LM) damps it:

$\mathbf{H}$ xấp xỉ ma trận Hessian của F mà không cần đạo hàm bậc hai của $\mathbf{r}$. Khi còn xa nghiệm, bước Gauss–Newton có thể đi quá đà, nên Levenberg–Marquardt (LM) giảm chấn nó:

$$\big(\mathbf{H} + \lambda\mathbf{D}\big)\,\boldsymbol{\delta} = -\mathbf{g}, \qquad \mathbf{D} = \operatorname{diag}(\mathbf{H})$$

A small $\lambda$ gives the fast Gauss–Newton step; a large $\lambda$ gives a short step along the scaled gradient, which lowers the cost if it is short enough. LM adapts $\lambda$ with the gain ratio, the actual decrease of the cost over the decrease that the linear model $L$ predicted:

$\lambda$ nhỏ cho bước Gauss–Newton nhanh; $\lambda$ lớn cho bước ngắn theo gradient đã chuẩn hóa, bước này làm giảm chi phí nếu đủ ngắn. LM điều chỉnh $\lambda$ theo tỉ số giữa mức giảm thực tế của chi phí và mức giảm mà mô hình tuyến tính $L$ dự đoán (gain ratio):

$$\varrho = \frac{F(\mathbf{x}) - F(\mathbf{x}\boxplus\boldsymbol{\delta})}{L(\mathbf{0}) - L(\boldsymbol{\delta})}, \qquad L(\boldsymbol{\delta}) = \tfrac{1}{2}\big\|\mathbf{r} + \mathbf{J}\boldsymbol{\delta}\big\|^2_{\mathbf{W}}$$

If $\varrho > 0$ the step is accepted and $\lambda$ shrinks; otherwise the step is rejected, $\lambda$ grows and the system is solved again. The demo below uses the simplest form of this rule: keep the step if the cost drops and divide $\lambda$ by 3, otherwise multiply $\lambda$ by 4. One iteration has five parts:

Nếu $\varrho > 0$, bước được chấp nhận và $\lambda$ giảm; ngược lại bước bị loại, $\lambda$ tăng và hệ được giải lại. Phần thử nghiệm bên dưới dùng dạng đơn giản nhất của quy tắc này: giữ bước nếu chi phí giảm và chia $\lambda$ cho 3, nếu không thì nhân $\lambda$ với 4. Mỗi vòng lặp gồm năm phần:

1. Linearize1. Tuyến tính hóaErrors $\mathbf{r}$ and Jacobian blocks at the current estimate.Sai số $\mathbf{r}$ và các khối Jacobian tại ước lượng hiện tại.

2. Build2. Lập hệ$\mathbf{H} = \mathbf{J}^\top\mathbf{W}\mathbf{J}$ and $\mathbf{g} = \mathbf{J}^\top\mathbf{W}\mathbf{r}$, block by block.$\mathbf{H} = \mathbf{J}^\top\mathbf{W}\mathbf{J}$ và $\mathbf{g} = \mathbf{J}^\top\mathbf{W}\mathbf{r}$, theo từng khối.

3. Reduce3. Rút gọnEliminate the points with the Schur complement.Khử các điểm bằng phần bù Schur.

4. Solve4. GiảiThe small camera system, then each point on its own.Hệ nhỏ của các camera, rồi từng điểm riêng rẽ.

5. Accept or reject5. Nhận hoặc loạiCompare the costs, update $\lambda$, repeat until the step is tiny.So sánh chi phí, cập nhật $\lambda$, lặp đến khi bước rất nhỏ.

Poses live on a manifoldTư thế nằm trên một đa tạp

A rotation matrix has 9 numbers but only 3 degrees of freedom. If LM added a step to the entries of $\mathbf{R}$, the result would no longer be a rotation. Instead, BA takes the step in the 6-dimensional tangent space of the pose and maps it back onto the group SE(3) with the exponential map. The notation $\boxplus$ above means exactly this update:

Ma trận quay có 9 phần tử nhưng chỉ có 3 bậc tự do. Nếu LM cộng một bước vào các phần tử của $\mathbf{R}$, kết quả sẽ không còn là phép quay. Thay vào đó, BA thực hiện bước trong không gian tiếp tuyến 6 chiều của tư thế rồi đưa nó trở lại nhóm SE(3) bằng ánh xạ mũ. Ký hiệu $\boxplus$ ở trên chính là phép cập nhật này:

$$\mathbf{T} \;\leftarrow\; \operatorname{Exp}(\boldsymbol{\delta\xi})\,\mathbf{T}, \qquad \boldsymbol{\delta\xi} = \begin{bmatrix}\boldsymbol{\delta\rho}\\ \boldsymbol{\delta\phi}\end{bmatrix} \in \mathbb{R}^6$$

Figure 2. The step $\boldsymbol{\delta\xi}$ is taken in the flat tangent space at $\mathbf{T}$ and mapped back onto the curved group by Exp. Adding the step to the matrix entries ($\mathbf{T} + \boldsymbol{\delta}$) would leave the group.Hình 2. Bước $\boldsymbol{\delta\xi}$ được thực hiện trong không gian tiếp tuyến phẳng tại $\mathbf{T}$ rồi được Exp đưa trở lại nhóm cong. Cộng bước vào các phần tử ma trận ($\mathbf{T} + \boldsymbol{\delta}$) sẽ rời khỏi nhóm.

Here $\boldsymbol{\delta\rho}$ is the translation and $\boldsymbol{\delta\phi}$ the rotation part. For the rotation, the exponential map is Rodrigues' formula, with $\theta = \|\boldsymbol{\phi}\|$ and $[\boldsymbol{\phi}]_\times$ the cross-product matrix:

Ở đây $\boldsymbol{\delta\rho}$ là phần tịnh tiến và $\boldsymbol{\delta\phi}$ là phần quay. Với phép quay, ánh xạ mũ chính là công thức Rodrigues, với $\theta = \|\boldsymbol{\phi}\|$ và $[\boldsymbol{\phi}]_\times$ là ma trận tích có hướng:

$$\operatorname{Exp}(\boldsymbol{\phi}) = \mathbf{I} + \frac{\sin\theta}{\theta}\,[\boldsymbol{\phi}]_\times + \frac{1-\cos\theta}{\theta^2}\,[\boldsymbol{\phi}]_\times^2$$

So the step has exactly 6 numbers per camera and 3 per point, and every update gives a valid pose. Most BA libraries update poses this way (Ceres through its manifolds, g²o and GTSAM through Lie-group updates) or use another minimal parametrization, such as the angle-axis vector of the BAL cameras. PyPose, on which BAE builds, provides these Lie-group operations in PyTorch.

Như vậy bước cập nhật có đúng 6 số cho mỗi camera và 3 số cho mỗi điểm, và mọi lần cập nhật đều cho ra một tư thế hợp lệ. Phần lớn thư viện BA cập nhật tư thế theo cách này (Ceres qua các đa tạp, g²o và GTSAM qua phép cập nhật trên nhóm Lie) hoặc dùng một cách tham số hóa tối thiểu khác, như vectơ trục-góc của camera trong BAL. PyPose, nền tảng của BAE, cung cấp các phép toán nhóm Lie này trong PyTorch.

The JacobiansCác ma trận Jacobian

Each error $\mathbf{r}_{ij}$ depends on one pose and one point, so each block row of $\mathbf{J}$ has just two non-zero blocks. Both follow from the chain rule through $\mathbf{P} = \mathbf{R}_i\mathbf{X}_j + \mathbf{t}_i$:

Mỗi sai số $\mathbf{r}_{ij}$ phụ thuộc vào một tư thế và một điểm, nên mỗi hàng khối của $\mathbf{J}$ chỉ có hai khối khác không. Cả hai đều suy ra từ quy tắc dây chuyền qua $\mathbf{P} = \mathbf{R}_i\mathbf{X}_j + \mathbf{t}_i$:

$$\frac{\partial\mathbf{r}_{ij}}{\partial\boldsymbol{\delta\xi}_i} = \frac{\partial\pi}{\partial\mathbf{P}}\,\frac{\partial\mathbf{P}}{\partial\boldsymbol{\delta\xi}_i}, \qquad \frac{\partial\mathbf{r}_{ij}}{\partial\mathbf{X}_j} = \frac{\partial\pi}{\partial\mathbf{P}}\,\mathbf{R}_i$$ $$\frac{\partial\pi}{\partial\mathbf{P}} = \begin{bmatrix} f_x/z & 0 & -f_x\,x/z^2\\ 0 & f_y/z & -f_y\,y/z^2\end{bmatrix}, \qquad \frac{\partial\mathbf{P}}{\partial\boldsymbol{\delta\xi}} = \begin{bmatrix}\mathbf{I}_3 & -[\mathbf{P}]_\times\end{bmatrix}$$

The last one comes from $\operatorname{Exp}(\boldsymbol{\delta\xi})\,\mathbf{P} \approx \mathbf{P} + \boldsymbol{\delta\rho} + \boldsymbol{\delta\phi}\times\mathbf{P}$ for a small step. The camera block is $2\times 6$ and the point block $2\times 3$. If the intrinsics are refined too, the camera block gets more columns: $2\times 9$ in the BAL benchmark, whose cameras also have a focal length and two distortion coefficients.

Đẳng thức cuối suy ra từ $\operatorname{Exp}(\boldsymbol{\delta\xi})\,\mathbf{P} \approx \mathbf{P} + \boldsymbol{\delta\rho} + \boldsymbol{\delta\phi}\times\mathbf{P}$ khi bước nhỏ. Khối camera có kích thước $2\times 6$ và khối điểm $2\times 3$. Nếu tinh chỉnh cả thông số nội, khối camera có thêm cột: $2\times 9$ trong bộ dữ liệu chuẩn BAL, nơi mỗi camera còn có tiêu cự và hai hệ số méo.

Sparsity and the Schur complementTính thưa và phần bù Schur

Order the unknowns with all camera parameters $\mathbf{c}$ first and all points $\mathbf{p}$ after them. The normal equations then have an arrowhead structure:

Sắp xếp các ẩn với mọi tham số camera $\mathbf{c}$ đứng trước và mọi điểm $\mathbf{p}$ đứng sau. Khi đó hệ phương trình chuẩn có cấu trúc hình mũi tên:

$$\begin{bmatrix}\mathbf{B} & \mathbf{E}\\ \mathbf{E}^\top & \mathbf{C}\end{bmatrix}\begin{bmatrix}\boldsymbol{\delta}_c\\ \boldsymbol{\delta}_p\end{bmatrix} = -\begin{bmatrix}\mathbf{g}_c\\ \mathbf{g}_p\end{bmatrix}$$

$\mathbf{B}$ is block-diagonal with one $6\times 6$ block per camera, $\mathbf{C}$ is block-diagonal with one $3\times 3$ block per point, and the block $\mathbf{E}_{ij}$ is non-zero only if camera i observes point j.

$\mathbf{B}$ là ma trận đường chéo khối với một khối $6\times 6$ cho mỗi camera, $\mathbf{C}$ là ma trận đường chéo khối với một khối $3\times 3$ cho mỗi điểm, còn khối $\mathbf{E}_{ij}$ chỉ khác không khi camera i quan sát điểm j.

Figure 3. Four cameras in a row and five points, each point seen by two or three neighbouring cameras. Each of the 12 observations fills one camera block and one point block of $\mathbf{J}$. In $\mathbf{H}$, $\mathbf{B}$ and $\mathbf{C}$ are block-diagonal and $\mathbf{E}$ records which camera sees which point. The reduced camera system $\mathbf{S}$ has a block wherever two cameras share a point; cameras 1 and 4 share none.Hình 3. Bốn camera nối tiếp nhau và năm điểm, mỗi điểm được hai hoặc ba camera kề nhau nhìn thấy. Mỗi quan sát trong 12 quan sát lấp đầy một khối camera và một khối điểm của $\mathbf{J}$. Trong $\mathbf{H}$, $\mathbf{B}$ và $\mathbf{C}$ là đường chéo khối, còn $\mathbf{E}$ ghi lại camera nào nhìn thấy điểm nào. Hệ camera rút gọn $\mathbf{S}$ có một khối ở mọi nơi hai camera chung một điểm; camera 1 và 4 không chung điểm nào.

Since $\mathbf{C}$ is block-diagonal, its inverse is just n small $3\times 3$ inverses. Eliminating the points gives the reduced camera system, whose matrix is the Schur complement $\mathbf{S}$ of $\mathbf{C}$; the points then follow one by one, by back-substitution (with the damping $\lambda\mathbf{D}$ added to $\mathbf{B}$ and $\mathbf{C}$):

Vì $\mathbf{C}$ là đường chéo khối, nghịch đảo của nó chỉ là n phép nghịch đảo $3\times 3$ nhỏ. Khử các điểm cho ta hệ camera rút gọn, có ma trận là phần bù Schur $\mathbf{S}$ của $\mathbf{C}$; sau đó các điểm được tính lần lượt bằng phép thế ngược (với số hạng giảm chấn $\lambda\mathbf{D}$ cộng vào $\mathbf{B}$ và $\mathbf{C}$):

$$\underbrace{\big(\mathbf{B} - \mathbf{E}\,\mathbf{C}^{-1}\mathbf{E}^\top\big)}_{\mathbf{S}}\;\boldsymbol{\delta}_c = -\mathbf{g}_c + \mathbf{E}\,\mathbf{C}^{-1}\mathbf{g}_p, \qquad \boldsymbol{\delta}_p = -\mathbf{C}^{-1}\big(\mathbf{g}_p + \mathbf{E}^\top\boldsymbol{\delta}_c\big)$$

The gain is large because there are far more points than cameras. In the BAL problem 49-7776 (Ladybug), 49 cameras with 9 parameters each and 7,776 points give 23,769 unknowns, but $\mathbf{S}$ is only $441\times 441$. With thousands of cameras even $\mathbf{S}$ is too large to factorize. Then the camera system is solved with preconditioned conjugate gradients (PCG), which needs only matrix–vector products built from the blocks of $\mathbf{J}$ and never forms $\mathbf{S}$, as studied in BAL and Multicore BA; the BAE example below uses PCG too.

Lợi ích rất lớn vì số điểm nhiều hơn số camera rất nhiều. Trong bài toán BAL 49-7776 (Ladybug), 49 camera với 9 tham số mỗi camera và 7.776 điểm cho 23.769 ẩn, nhưng $\mathbf{S}$ chỉ có kích thước $441\times 441$. Khi có hàng nghìn camera, ngay cả $\mathbf{S}$ cũng quá lớn để phân rã. Khi đó hệ camera được giải bằng phương pháp gradient liên hợp có tiền điều kiện (PCG), chỉ cần các phép nhân ma trận với vectơ dựng từ các khối của $\mathbf{J}$ mà không bao giờ phải lập $\mathbf{S}$, như đã được nghiên cứu trong BAL và Multicore BA; ví dụ BAE bên dưới cũng dùng PCG.

Try it: bundle adjustment in 2DThử nghiệm: tối ưu bó trong 2D

The same algorithm in a flat world, small enough to run in your browser. Six cameras on an arc look at 30 points, and each camera measures one number per point: where it appears on the camera's one-dimensional image. Grey shapes are the truth and blue ones the estimate, which starts perturbed. Each LM step builds $\mathbf{J}$ and $\mathbf{H}$, eliminates the points with the Schur complement and solves for the step, as above; the matrices below the scene are the real ones of the current step.

Cùng thuật toán đó trong một thế giới phẳng, đủ nhỏ để chạy trong trình duyệt. Sáu camera đặt trên một cung tròn nhìn vào 30 điểm, và mỗi camera đo một con số cho mỗi điểm: vị trí của điểm trên ảnh một chiều của camera. Hình màu xám là giá trị thật, hình màu xanh là ước lượng, được khởi tạo lệch đi. Mỗi bước LM lập $\mathbf{J}$ và $\mathbf{H}$, khử các điểm bằng phần bù Schur rồi giải tìm bước, như ở trên; các ma trận bên dưới cảnh là ma trận thật của bước hiện tại.

10% outliers10% điểm ngoại lai

Kernel (δ = 3 px):Hàm bền vững (δ = 3 px):

estimateước lượng truthgiá trị thật outlier measurementphép đo ngoại lai held fixed (gauge)giữ cố định (gauge)

camera block (B, S)khối camera (B, S) point block (C)khối điểm (C) camera–point coupling (E)liên kết camera–điểm (E)

In 2D a camera has a position $\mathbf{c}$ and a heading $\theta$. With the viewing direction $\mathbf{d} = (\cos\theta, \sin\theta)$ and the sideways direction $\mathbf{l} = (-\sin\theta, \cos\theta)$, a point $\mathbf{p}$ has depth $z$ and sideways offset $x$, and is measured at

Trong 2D, một camera có vị trí $\mathbf{c}$ và hướng nhìn $\theta$. Với hướng nhìn $\mathbf{d} = (\cos\theta, \sin\theta)$ và hướng ngang $\mathbf{l} = (-\sin\theta, \cos\theta)$, một điểm $\mathbf{p}$ có độ sâu $z$ và độ lệch ngang $x$, và được đo tại

$$z = (\mathbf{p}-\mathbf{c})\cdot\mathbf{d}, \qquad x = (\mathbf{p}-\mathbf{c})\cdot\mathbf{l}, \qquad u = f\,\frac{x}{z} + \varepsilon, \quad \varepsilon \sim \mathcal{N}(0, \sigma^2)$$

with $f = 300$ px, $\sigma = 0.5$ px and a 60° field of view. There are 3 unknowns per camera and 2 per point. Camera 1 is held fixed and camera 2 may only turn, which removes the gauge freedom explained below. That leaves 13 camera and 60 point unknowns, so $\mathbf{J}$ is $148\times 73$, $\mathbf{H}$ is $73\times 73$ and the camera system $\mathbf{S}$ only $13\times 13$. What to look for:

với $f = 300$ px, $\sigma = 0{,}5$ px và trường nhìn 60°. Mỗi camera có 3 ẩn và mỗi điểm có 2 ẩn. Camera 1 được giữ cố định và camera 2 chỉ được xoay, nhằm loại bỏ bậc tự do gauge được giải thích bên dưới. Còn lại 13 ẩn camera và 60 ẩn điểm, nên $\mathbf{J}$ có kích thước $148\times 73$, $\mathbf{H}$ là $73\times 73$ và hệ camera $\mathbf{S}$ chỉ là $13\times 13$. Những điều nên quan sát:

Clean data. Least squares converges in 5 or 6 iterations from each of 30 starts we tried, from 22–43 px to 0.28 px RMS. That is a little below the 0.45 px RMS of the simulated noise, because the 73 unknowns absorb part of it. The points end 1.5 cm from the truth.
Outliers. With outliers on, each measurement is replaced by a random value with probability 10% (here 11 of the 148), and least squares is pulled away: from the default start, the inliers end at 20.4 px RMS and the points 2.17 m from the truth. Huber caps the pull of each outlier (9.6 px, 0.77 m); Cauchy lets it fall back towards zero (1.6 px, 0.11 m).
Starts matter for Cauchy. Over 30 starts, Cauchy always gave the smallest point error, but between 0.09 and 0.40 m, while Huber reached 0.77 m every time: Cauchy's kernel is not convex, so where it ends depends more on where it starts.
The matrices. $\mathbf{J}$ has at most two blocks per row (one if the camera is held fixed), $\mathbf{H}$ is an arrowhead, and $\mathbf{S}$ is small but dense, because every pair of cameras here sees some common points.

Dữ liệu sạch. Bình phương tối thiểu hội tụ sau 5 hoặc 6 vòng lặp với cả 30 lần khởi tạo chúng tôi đã thử, từ 22–43 px xuống 0,28 px RMS. Con số này thấp hơn một chút so với nhiễu mô phỏng 0,45 px RMS, vì 73 ẩn số đã hấp thụ một phần nhiễu. Các điểm cuối cùng cách giá trị thật 1,5 cm.
Điểm ngoại lai. Khi bật điểm ngoại lai, mỗi phép đo bị thay bằng một giá trị ngẫu nhiên với xác suất 10% (ở đây là 11 trong 148), và bình phương tối thiểu bị kéo lệch: từ khởi tạo mặc định, các điểm đúng kết thúc ở 20,4 px RMS và các điểm cách giá trị thật 2,17 m. Huber giới hạn lực kéo của mỗi điểm ngoại lai (9,6 px; 0,77 m); Cauchy để lực kéo đó giảm dần về 0 (1,6 px; 0,11 m).
Khởi tạo quan trọng với Cauchy. Qua 30 lần khởi tạo, Cauchy luôn cho sai số điểm nhỏ nhất nhưng dao động từ 0,09 đến 0,40 m, trong khi Huber lần nào cũng đạt 0,77 m: hàm Cauchy không lồi, nên nơi nó kết thúc phụ thuộc nhiều hơn vào nơi nó bắt đầu.
Các ma trận. $\mathbf{J}$ có nhiều nhất hai khối trên mỗi hàng (một khối nếu camera được giữ cố định), $\mathbf{H}$ có hình mũi tên, còn $\mathbf{S}$ nhỏ nhưng dày đặc, vì ở đây mọi cặp camera đều có điểm chung.

A teaching toy, not a benchmark: one-dimensional images, known correspondences, a fixed start and 30 points. The numbers above come from running this page's code (ba-core.js) in Node.Đây là mô hình minh họa, không phải phép đo hiệu năng: ảnh một chiều, cặp tương ứng đã biết, khởi tạo cố định và 30 điểm. Các con số ở trên có được khi chạy chính mã của trang này (ba-core.js) trong Node.

Robust kernelsHàm bền vững

Least squares assumes Gaussian errors. In the squared cost, one wrong match with a 100 px error weighs as much as 10,000 correct matches with a 1 px error. Robust kernels grow more slowly for large errors. With the squared error $e$ and a threshold $\delta$:

Bình phương tối thiểu giả định sai số có phân phối Gauss. Trong hàm chi phí bình phương, một cặp khớp sai có sai số 100 px nặng bằng 10.000 cặp khớp đúng có sai số 1 px. Các hàm bền vững tăng chậm hơn khi sai số lớn. Với bình phương sai số $e$ và ngưỡng $\delta$:

$$\rho_{\text{Huber}}(e) = \begin{cases} e, & e \le \delta^2\\ 2\delta\sqrt{e} - \delta^2, & e > \delta^2\end{cases} \qquad\qquad \rho_{\text{Cauchy}}(e) = \delta^2\log\!\Big(1 + \frac{e}{\delta^2}\Big)$$

LM handles them by iteratively reweighted least squares (IRLS): each iteration solves the normal equations of plain least squares with one weight per error, recomputed from the current errors:

LM xử lý chúng bằng bình phương tối thiểu có trọng số lặp lại (IRLS): mỗi vòng lặp giải hệ phương trình chuẩn của bình phương tối thiểu thông thường với một trọng số cho mỗi sai số, được tính lại từ sai số hiện tại:

$$w_{ij} = \rho'(e_{ij}), \qquad \mathbf{H} = \sum_{(i,j)} w_{ij}\,\mathbf{J}_{ij}^\top\boldsymbol{\Sigma}_{ij}^{-1}\mathbf{J}_{ij}, \qquad \mathbf{g} = \sum_{(i,j)} w_{ij}\,\mathbf{J}_{ij}^\top\boldsymbol{\Sigma}_{ij}^{-1}\mathbf{r}_{ij}$$ $$w_{\text{Huber}} = \min\!\Big(1,\, \frac{\delta}{\sqrt{e}}\Big), \qquad w_{\text{Cauchy}} = \frac{1}{1 + e/\delta^2}$$

With Huber, the pull of an error beyond the threshold, its weight times its size, stays at $\delta$; with Cauchy, it falls back towards zero, so far outliers are almost ignored. Huber is convex as a function of the residual, while Cauchy and other redescending kernels such as Tukey or Geman–McClure reject outliers more completely but add local minima. The threshold sets what counts as large, in units of the measurement noise: ORB-SLAM, for example, uses Huber with $\delta^2 = 5.99$, the 95% point of the $\chi^2$ distribution with 2 degrees of freedom. Systems also remove observations with large errors between BA runs, as COLMAP does.

Với Huber, lực kéo của một sai số vượt ngưỡng, tức trọng số nhân với độ lớn sai số, giữ nguyên ở $\delta$; với Cauchy, lực kéo đó giảm dần về 0, nên các điểm ngoại lai ở xa gần như bị bỏ qua. Huber là hàm lồi theo phần dư, còn Cauchy và các hàm giảm dần khác như Tukey hay Geman–McClure loại điểm ngoại lai triệt để hơn nhưng sinh thêm cực tiểu cục bộ. Ngưỡng quyết định thế nào là lớn, tính theo đơn vị nhiễu đo: ví dụ ORB-SLAM dùng Huber với $\delta^2 = 5{,}99$, điểm 95% của phân phối $\chi^2$ với 2 bậc tự do. Các hệ thống cũng loại bỏ các quan sát có sai số lớn giữa các lần chạy BA, như COLMAP.

Gauge freedom: what images cannot tellBậc tự do gauge: điều mà ảnh không cho biết

Images do not change if the whole scene and all cameras are moved, turned and scaled together. For any similarity transform with scale $s$, rotation $\mathbf{R}$ and translation $\mathbf{t}$, and with $\mathbf{c}_i$ the centre of camera i:

Ảnh không thay đổi nếu toàn bộ cảnh và mọi camera cùng được dịch chuyển, xoay và co giãn. Với mọi phép biến đổi đồng dạng có hệ số tỉ lệ $s$, phép quay $\mathbf{R}$ và phép tịnh tiến $\mathbf{t}$, và với $\mathbf{c}_i$ là tâm của camera i:

$$\mathbf{X}_j' = s\mathbf{R}\mathbf{X}_j + \mathbf{t}, \quad \mathbf{c}_i' = s\mathbf{R}\mathbf{c}_i + \mathbf{t}, \quad \mathbf{R}_i' = \mathbf{R}_i\mathbf{R}^\top \quad\Longrightarrow\quad \mathbf{P}_{ij}' = s\,\mathbf{P}_{ij}, \quad \pi(\mathbf{P}_{ij}') = \pi(\mathbf{P}_{ij})$$

Figure 4. Two reconstructions that differ by a similarity transform give exactly the same images, so they have the same reprojection errors. BA has to be told which one to pick.Hình 4. Hai bản tái tạo khác nhau một phép biến đổi đồng dạng cho ra đúng cùng một bộ ảnh, nên chúng có cùng sai số chiếu lại. Phải chỉ cho BA biết nên chọn bản nào.

With a single moving camera this leaves 7 directions that no image can observe: 3 for position, 3 for rotation and 1 for scale. $\mathbf{H}$ has a 7-dimensional null space, and the problem has infinitely many solutions. Stereo or depth fixes the scale (6 left); an IMU also makes roll and pitch observable through gravity, which leaves 4 (position and heading). BA removes the gauge by fixing the first camera and the scale, for example the distance to the second camera, or by adding a weak prior. LM's damping also keeps the system solvable, but the solution can then drift along the null space.

Với một camera đơn chuyển động, còn lại 7 hướng mà không ảnh nào quan sát được: 3 cho vị trí, 3 cho phép quay và 1 cho tỉ lệ. $\mathbf{H}$ có không gian rỗng (null space) 7 chiều, và bài toán có vô số nghiệm. Camera stereo hoặc cảm biến độ sâu cố định được tỉ lệ (còn 6); IMU còn giúp quan sát được góc nghiêng ngang và dọc nhờ trọng lực, chỉ còn lại 4 (vị trí và hướng). BA loại bỏ gauge bằng cách cố định camera đầu tiên và tỉ lệ, ví dụ khoảng cách đến camera thứ hai, hoặc thêm một tiền nghiệm yếu. Số hạng giảm chấn của LM cũng giữ cho hệ giải được, nhưng khi đó nghiệm có thể trôi dọc theo không gian rỗng.

In the 2D demo the gauge has 4 directions: 2 for position, 1 for rotation and 1 for scale. We checked it: with every camera free, $\mathbf{H}$ at the true solution has exactly four eigenvalues that are zero up to rounding (below $10^{-17}$ of the largest). Fixing camera 1 and the position of camera 2 removes them, with one number to spare, which does no harm here because the fixed values are the true ones.

Trong thử nghiệm 2D, gauge có 4 hướng: 2 cho vị trí, 1 cho phép quay và 1 cho tỉ lệ. Chúng tôi đã kiểm tra: khi mọi camera đều tự do, $\mathbf{H}$ tại nghiệm thật có đúng bốn trị riêng bằng 0 trong phạm vi sai số làm tròn (nhỏ hơn $10^{-17}$ lần trị riêng lớn nhất). Cố định camera 1 và vị trí của camera 2 loại bỏ chúng, dư ra một số, điều này vô hại ở đây vì các giá trị được cố định là giá trị thật.

Fixing the gauge does not make the problem well conditioned. The next smallest eigenvalues of the same $\mathbf{H}$ are only $3\times 10^{-5}$ and $5\times 10^{-5}$ of the largest: weak directions that change the images very little, along which noise moves the solution more. In 3D, a classic case is a narrow field of view, where a small rotation of a camera and a small sideways move give almost the same image motion.

Cố định gauge không có nghĩa là bài toán được điều kiện hóa tốt. Các trị riêng nhỏ tiếp theo của cùng ma trận $\mathbf{H}$ chỉ bằng $3\times 10^{-5}$ và $5\times 10^{-5}$ lần trị riêng lớn nhất: đó là các hướng yếu, làm ảnh thay đổi rất ít, và nhiễu đẩy nghiệm đi xa hơn theo các hướng đó. Trong 3D, một trường hợp kinh điển là trường nhìn hẹp, khi một phép xoay nhỏ của camera và một dịch chuyển ngang nhỏ tạo ra chuyển động ảnh gần như giống nhau.

Where bundle adjustment runsTối ưu bó được dùng ở đâu

SystemHệ thống	What BA does thereVai trò của BA
COLMAP	Incremental structure from motion: a local BA after each new image and a global BA as the model grows, on Ceres.Cấu trúc từ chuyển động tăng dần: BA cục bộ sau mỗi ảnh mới và BA toàn cục khi mô hình lớn dần, chạy trên Ceres.
GLOMAP	Global structure from motion: positions all cameras and points at once, then refines them with BA.Cấu trúc từ chuyển động toàn cục: định vị mọi camera và điểm cùng lúc, rồi tinh chỉnh bằng BA.
ORB-SLAM	Local BA over a keyframe and its covisible keyframes; a full BA after a loop closure; on g²o.BA cục bộ trên một khung hình khóa và các khung hình khóa cùng quan sát; BA toàn bộ sau khi đóng vòng; chạy trên g²o.
DSO	Photometric BA over a window of keyframes (poses, inverse depths, brightness); old keyframes are marginalized with the Schur complement.BA trắc quang trên một cửa sổ khung hình khóa (tư thế, nghịch đảo độ sâu, độ sáng); các khung hình khóa cũ được biên hóa bằng phần bù Schur.
VINS-Mono, OKVIS, Basalt	Visual-inertial BA in a sliding window, with IMU error terms between keyframes and marginalization of old states.BA thị giác-quán tính trong cửa sổ trượt, với các số hạng sai số IMU giữa các khung hình khóa và phép biên hóa các trạng thái cũ.
DROID-SLAM	A dense BA layer inside a recurrent network: poses and per-pixel inverse depths, solved with the Schur complement on the GPU and differentiable.Một lớp BA dày đặc bên trong mạng hồi quy: tư thế và nghịch đảo độ sâu của từng điểm ảnh, giải bằng phần bù Schur trên GPU và khả vi.
VGGSfM	A differentiable BA layer at the end of a learned structure-from-motion pipeline.Một lớp BA khả vi ở cuối một hệ cấu trúc từ chuyển động học được.

BAE: bundle adjustment in PyTorch's eager modeBAE: tối ưu bó trong chế độ eager của PyTorch

Learned 3D pipelines such as DROID-SLAM, VGGSfM or VGGT run on GPUs in PyTorch, while the standard BA solvers (Ceres, g²o, GTSAM) are C++ libraries that run on CPUs. Moving data between the two costs time, cuts the gradient path and makes the code harder to change and debug. PyTorch's generic Jacobian routines do not solve this, because they return dense matrices. For BAL problem 49-7776, with 31,843 observations, a dense $\mathbf{J}$ would have $63{,}686 \times 23{,}769 \approx 1.5$ billion entries (6 GB in 32-bit floats), of which only 764,232 (0.05%) are non-zero.

Các hệ 3D học sâu như DROID-SLAM, VGGSfM hay VGGT chạy trên GPU trong PyTorch, trong khi các bộ giải BA tiêu chuẩn (Ceres, g²o, GTSAM) là thư viện C++ chạy trên CPU. Chuyển dữ liệu qua lại giữa hai bên tốn thời gian, cắt đứt đường truyền gradient và khiến mã khó thay đổi, khó gỡ lỗi. Các hàm tính Jacobian tổng quát của PyTorch không giải quyết được việc này, vì chúng trả về ma trận dày đặc. Với bài toán BAL 49-7776 gồm 31.843 quan sát, một ma trận $\mathbf{J}$ dày đặc sẽ có $63.686 \times 23.769 \approx 1{,}5$ tỉ phần tử (6 GB với số thực 32 bit), trong đó chỉ 764.232 phần tử (0,05%) khác không.

BAE (Bundle Adjustment in the Eager mode; Zhan, Xu, Fang, Wei, Hu and Wang, IEEE T-RO 2026) is a PyTorch library for exact second-order optimization, aimed at BA and pose-graph optimization. It keeps everything on the GPU in PyTorch's default eager execution: you write the error of one observation as ordinary PyTorch code, and BAE does the rest.

BAE (Bundle Adjustment in the Eager mode; Zhan, Xu, Fang, Wei, Hu và Wang, IEEE T-RO 2026) là thư viện PyTorch cho tối ưu bậc hai chính xác, hướng tới BA và tối ưu đồ thị tư thế. Nó giữ mọi thứ trên GPU trong chế độ thực thi eager mặc định của PyTorch: bạn viết sai số của một quan sát bằng mã PyTorch thông thường, phần còn lại do BAE đảm nhận.

1. Errors in parallel1. Sai số song songThe error of one observation, run for all observations at once and indexed by camera and point (cidx, pidx).Sai số của một quan sát, chạy cho mọi quan sát cùng lúc, đánh chỉ số theo camera và điểm (cidx, pidx).

2. Sparse Jacobian2. Jacobian thưaAutomatic differentiation that knows each error touches one camera and one point returns only those blocks.Vi phân tự động biết mỗi sai số chỉ chạm tới một camera và một điểm, nên chỉ trả về các khối đó.

3. Sparse algebra3. Đại số thưaSparse block matrices with custom CUDA kernels; no dense matrix is formed.Ma trận thưa theo khối với các kernel CUDA tự viết; không lập ma trận dày đặc nào.

4. Second-order step4. Bước bậc haiLevenberg–Marquardt with a trust region; the step from PCG, the Schur complement (less memory) or NVIDIA cuDSS.Levenberg–Marquardt với vùng tin cậy; bước giải bằng PCG, phần bù Schur (ít bộ nhớ hơn) hoặc NVIDIA cuDSS.

Figure 5. Generic automatic differentiation computes a dense Jacobian that is almost all zeros (left). BAE keeps only the blocks of each observation, next to the camera and point indices that say where they belong (right).Hình 5. Vi phân tự động tổng quát tính một ma trận Jacobian dày đặc mà gần như toàn số 0 (trái). BAE chỉ giữ các khối của mỗi quan sát, kèm chỉ số camera và điểm cho biết chúng thuộc về đâu (phải).

The BAL example from the BAE README, slightly shortened and commented. The projection uses the BAL camera model (an SE(3) pose, then focal length $f$ and radial distortion $k_1, k_2$); sjac=True marks parameters whose Jacobian is kept sparse, and @psjac (parallel-for sparse Jacobian) marks a function that is applied to each observation independently:

Ví dụ BAL trong README của BAE, rút gọn một chút và có chú thích. Phép chiếu dùng mô hình camera của BAL (một tư thế SE(3), rồi tiêu cự $f$ và méo xuyên tâm $k_1, k_2$); sjac=True đánh dấu các tham số có Jacobian được giữ ở dạng thưa, còn @psjac (parallel-for sparse Jacobian) đánh dấu một hàm được áp dụng độc lập cho từng quan sát:

import torch
import pypose as pp
from pypose.autograd.function import psjac
from bae.optim import LM
from bae.utils.pysolvers import PCG

class Reproj(torch.nn.Module):
    def __init__(self, camera_params, points):
        super().__init__()
        self.pose = pp.Parameter(camera_params, sjac=True)  # per camera: SE(3) as 7 numbers, then f, k1, k2
        self.points = pp.Parameter(points, sjac=True)       # per point: x, y, z
        self.pose.trim_SE3_grad = True

    @psjac
    def project(points, camera_params):                     # one observation, run for all in parallel
        p = pp.SE3(camera_params[..., :7]).Act(points)      # world to camera
        p = -p[..., :2] / p[..., [2]]                       # BAL cameras look down -z
        f, k1, k2 = camera_params[..., [-3]], camera_params[..., [-2]], camera_params[..., [-1]]
        n = torch.sum(p**2, dim=-1, keepdim=True)
        return p * (1 + k1 * n + k2 * n**2) * f             # radial distortion, focal length

    def forward(self, observes, cidx, pidx):                # residuals of all observations
        return Reproj.project(self.points[pidx], self.pose[cidx]) - observes

model = Reproj(camera_params, points).to("cuda")            # BAL problem-49-7776 in the README
optimizer = LM(model, strategy=pp.optim.strategy.TrustRegion(up=2.0, down=0.5**4),
               solver=PCG(tol=1e-4, maxiter=250), reject=30)
for it in range(20):
    loss = optimizer.step({"observes": observes, "cidx": cidx, "pidx": pidx})

Adapted from the pypose/bae README (Apache-2.0); the data loading is left out.Phỏng theo README của pypose/bae (giấy phép Apache-2.0); bỏ phần nạp dữ liệu.

Speed. The paper reports average speedups of 18.5×, 22× and 23× over GTSAM, g²o and Ceres across its benchmarks (numbers from the paper, not re-measured here).
One program. BA becomes ordinary PyTorch code built from differentiable operations, so it runs in the same program as the networks around it, without copying data to a CPU solver.
Where it is used. The authors' VGGT fork uses BAE as an optional BA backend before exporting a COLMAP reconstruction; BAE also refines camera poses predicted by Depth Anything 3, and powers BA and global positioning in InstantSfM. Its basic functions are in PyPose's Levenberg–Marquardt optimizer as a sparse backend since PyPose 0.9.5.

Tốc độ. Bài báo báo cáo mức tăng tốc trung bình 18,5×, 22× và 23× so với GTSAM, g²o và Ceres trên các bộ dữ liệu thử nghiệm của họ (số liệu từ bài báo, chưa được đo lại ở đây).
Một chương trình. BA trở thành mã PyTorch thông thường dựng từ các phép toán khả vi, nên nó chạy trong cùng chương trình với các mạng nơ-ron xung quanh, không cần sao chép dữ liệu sang một bộ giải trên CPU.
Nơi được dùng. Bản VGGT của nhóm tác giả dùng BAE làm bộ BA tùy chọn trước khi xuất bản tái tạo COLMAP; BAE cũng tinh chỉnh tư thế camera do Depth Anything 3 dự đoán, và đảm nhận BA cùng định vị toàn cục trong InstantSfM. Các chức năng cơ bản của nó có trong bộ tối ưu Levenberg–Marquardt của PyPose dưới dạng backend thưa từ PyPose 0.9.5.

CodeMã nguồn · project pagetrang dự án · arXiv 2409.12190 · IEEE T-RO

A reading mapBản đồ đọc

: papersbài báo
: lines of workhướng nghiên cứu
: branchesnhánh
: yearsgiai đoạn

Key papers, from the theory to GPU solvers and differentiable layers; a selection, not a complete list. Pick a line of work or a paper to see what it did and open it.Các bài báo chính, từ lý thuyết đến bộ giải trên GPU và lớp khả vi; một tuyển chọn, không phải danh sách đầy đủ. Chọn một hướng nghiên cứu hoặc một bài báo để xem nó làm gì và mở bài báo.

Bundle adjustment, explained: from reprojection error to GPU solversGiải thích tối ưu bó: từ sai số chiếu lại đến bộ giải trên GPU