一、理論
1.?狀態估計的概率解釋
我們來深入探討一下視覺SLAM中狀態估計的概率解釋。這可以說是理解現代SLAM算法（尤其是后端優化）的基石
1. 問題的核心：不確定性
SLAM（同步定位與建圖）的本質是在一個未知環境中，用一個移動的傳感器（如相機）來同時估計自身的運動軌跡（定位）和環境的結構（建圖）。
這個過程充滿了不確定性（Uncertainty）：
- 運動模型不準
：我們通過輪式里程計、IMU或幀間匹配來估計相機的運動，但這些估計都有誤差，會隨著時間累積。
- 觀測模型不準
：相機在拍攝環境中的特征點（路標點）時，由于光照、相機分辨率、圖像噪聲等因素，我們提取到的像素坐標也不是100%精確的。
概率論正是用來數學化地描述和處理這種不CETP的完美工具。因此，SLAM問題被自然地建模成一個概率問題。
2.?線性系統和KF
我們接著上一個話題，從更基礎的線性系統和卡爾曼濾波 (Kalman Filter, KF)?開始。這不僅是理解EKF-SLAM的必要前奏，其本身也是控制論和信號處理領域最璀璨的成果之一。
理解了KF，你就能明白EKF是如何通過“作弊”（線性化）來處理非線性問題的。
3.?非線性系統與EKF
我們現在進入了從理論到實踐的關鍵一步：非線性系統與擴展卡爾曼濾波 (EKF)。這正是為了解決視覺SLAM等真實世界問題而對經典卡爾曼濾波進行的“擴展”。
總結一下變化：
在計算狀態均值時，我們使用原始的非線性函數?
f?和?h，因為這是對真實物理過程的最佳描述。在計算協方差（不確定性）的傳播和更新時，我們使用雅可比矩陣?
F?和?H，因為協方差的演化需要一個線性的框架。
4.?BA與圖優化
我們現在已經到達了現代視覺SLAM技術的核心——捆集調整 (Bundle Adjustment, BA) 與圖優化 (Graph Optimization)。這可以說是視覺SLAM后端優化中的“圣杯”，是獲得高精度軌跡和地圖的關鍵。
核心關系：一般與特殊
首先，我們來理清這兩個概念的關系：
- 圖優化 (Graph Optimization)
：是一種更廣泛、更通用的優化理論框架。它將一個復雜的優化問題建模成一個圖（Graph），其中節點代表待優化的變量，邊代表變量之間的約束（或誤差）。它的目標是調整節點的值，使得所有邊所代表的誤差之和最小。
- 捆集調整 (Bundle Adjustment, BA)
：是圖優化理論在視覺SLAM領域中最核心、最成功的具體應用。它專門用來優化相機位姿和三維地圖點，其誤差項是特定的“重投影誤差”。
所以，你可以這樣理解：BA 是一種特殊的圖優化。
5.?投影模型與BA代價函數
這是理解BA如何工作的關鍵一步。我們將把這個過程分解成兩個部分：
- 幾何部分
：一個三維點是如何在物理上變成一個二維像素的？這就是投影模型 (Projection Model)。
- 數學部分
：如何用一個數學公式來衡量我們的估計（位姿和地圖點）與真實觀測之間的差距？這就是BA代價函數 (Cost Function)。
上圖中，藍色點是實際觀測，紅色叉是三維點根據當前估計重投影的位置，它們之間的像素距離就是重投影誤差。
上面的解釋為了簡化，使用的是最理想的針孔相機模型 (Pinhole Camera Model)，它假設光線沿直線穿過一個無限小的孔。
然而，現實世界中的相機鏡頭是由多片凸透鏡和凹透鏡組合而成的，這會導致光線在通過鏡頭時發生彎曲，使得最終成像的位置與理想模型預測的位置有偏差。這種偏差就是相機畸變 (Camera Distortion)。
不考慮畸變，SLAM系統的精度會受到嚴重影響，尤其是在使用廣角或魚眼鏡頭時。因此，在實際的BA中，畸變模型是必須考慮的一部分。
6.?BA的求解
我們來深入剖析BA的求解過程。這是一個將理論（代價函數）轉化為實際代碼和高效計算的關鍵環節。
7.?魯棒核函數
這是一個非常深入且至關重要的話題。在真實的SLAM應用中，僅僅使用標準的最小二乘法（即BA代價函數）是遠遠不夠的，因為我們無法保證所有的數據都是完美的。魯棒核函數 (Robust Kernel Function)，也常被稱為魯棒代價函數 (Robust Cost Function)，正是為了解決這個問題而生的。
特性 (Feature) | Huber 核函數 (Huber) | Cauchy 核函數 (Cauchy) | Geman-McClure 核函數 (Geman-McClure) | Tukey 核函數 (Tukey) |
|---|---|---|---|---|
| 魯棒性 (Robustness) | 中等 (Medium) | 強 (Strong) | 強 (Strong) | 最強 (Strongest) |
| 處理大誤差方式 (How Large Errors are Handled) | 線性懲罰 (Linear Penalty) | 權重衰減 (Weight Decay) | 權重衰減 (Weight Decay) | 完全忽略 (Completely Ignored) |
| 凸性 (Convexity) | 條件凸 (Conditionally Convex) | 非凸 (Non-convex) | 非凸 (Non-convex) | 非凸 (Non-convex) |
| 可導性 (Differentiability) | 連續可導 (Continuously Diff.) | 連續可導 (Continuously Diff.) | 連續可導 (Continuously Diff.) | 某些點不可導 (Not Diff. at some points) |
| 參數 (Parameter) | δ | c | c | c |
| 適用場景 (Applicable Scenarios) | 對異常值有一定容忍，但又不想完全拋棄其信息時 | 異常值較多且希望強魯棒性時 | 類似 Cauchy 核函數，異常值較多時 | 異常值非常離散，希望完全忽略其影響 |
在實際應用中，選擇哪種核函數取決于具體的數據特性和對魯棒性的需求。通常，Huber 核函數是一個很好的起點，因為它在魯棒性和優化難度之間取得了較好的平衡。如果異常值非常嚴重且需要強烈的抑制，那么 Cauchy、Geman-McClure 或 Tukey 核函數可能更合適，但需要注意非凸性可能帶來的局部最優問題。在機器人和計算機視覺的位姿估計 (pose estimation) 等問題中，由于存在大量錯誤匹配 (mismatches) 導致的異常值，這些魯棒核函數扮演著至關重要的角色。
二、代碼筆記
1.?bundle_adjustment_ceres.cpp
基于 Ceres 的稀疏 Schur 結構 Bundle Adjustment 囊括了：
BA 的變量與觀測模型；
重投影誤差與魯棒損失；
稀疏 Schur 求解策略；
在 Ceres 中的配置與求解流程。
該程序是一個使用Ceres Solver庫來解決大規模光束法平差 (Bundle Adjustment, BA)問題的完整示例。BA是視覺SLAM和三維重建中至關重要的后端優化步驟。
程序的主要工作流程如下：
數據加載與預處理：
多個相機的初始位姿和內參（如焦距、畸變系數）。
大量三維空間點的初始坐標。
大量的觀測數據，即哪個相機看到了哪個3D點，以及該點在該相機圖像上的2D像素坐標。
程序從一個特定格式（BAL格式）的文件中加載一個BA問題。這個文件包含了：
為了提高數值穩定性，程序對加載的數據進行了歸一化。
為了模擬真實的優化場景，程序對完美的初始數據添加了隨機擾動，創造了一個需要被優化的非理想狀態。
構建優化問題 (
SolveBA函數)：- 代價函數 (Cost Function)
：一個
SnavelyReprojectionError實例，它定義了如何根據給定的相機和3D點計算重投影誤差。 - 損失函數 (Loss Function)
：一個
HuberLoss實例。這是一個魯棒核函數，用于減小由錯誤匹配或外點（outliers）引起的巨大誤差對整體優化的不良影響。 - 待優化參數塊 (Parameter Blocks)
：指向該觀測所關聯的相機參數和3D點參數的內存地址。Ceres將通過調整這些內存中的數值來最小化代價函數。
程序的核心是構建一個Ceres的
Problem對象。它遍歷每一條觀測數據（一個
camera-point-2D_observation的組合）。對于每條觀測，它創建一個殘差塊 (Residual Block)?并添加到
Problem中。每個殘差塊包含：
- 代價函數 (Cost Function)
求解與結果輸出：
程序配置了Ceres的求解器。最關鍵的配置是選擇了
SPARSE_SCHUR作為線性求解器。這種求解器利用了BA問題中雅可比矩陣的稀疏結構（一個相機只看到一部分點，一個點只被一部分相機看到），通過舒爾補 (Schur Complement)技巧，極大地提高了求解大規模BA問題的效率。調用
ceres::Solve()啟動優化過程。Ceres會迭代地調整所有相機參數和所有3D點坐標，以聯合最小化所有觀測的重投影誤差之和。優化完成后，程序打印詳細的報告，并將優化前后的相機位姿和3D點云保存為PLY文件，以便通過MeshLab等工具進行可視化比較。
2.bundle_adjustment_g2o.cpp
擴展相機內參的 g2o Bundle Adjustment總結：
相機位姿與內參的聯合參數化；
畸變與透視投影模型；
魯棒殘差定義與邊的構造；
基于 g2o 的 Levenberg–Marquardt 優化流程。
該程序使用g2o (General Graph Optimization)庫來解決與前一個Ceres示例完全相同的大規模光束法平差 (Bundle Adjustment, BA)問題。它展示了如何使用g2o的框架將BA問題建模為一個圖，并進行高效求解。
程序的主要工作流程如下：
數據加載與預處理：與Ceres示例完全相同，程序首先從BAL文件中加載相機、地圖點和觀測數據，并對其進行歸一化和擾動。
定義圖的元素：這是使用g2o的核心步驟。
EdgeProjection：這是一個二元邊，連接一個
VertexPoseAndIntrinsics和一個VertexPoint。它代表了一個重投影誤差的約束。computeError函數中定義了如何根據連接的兩個頂點的當前估計值來計算預測的2D投影，并與真實的2D觀測值比較得到誤差。本例中，該邊沒有手動實現雅可比矩陣，而是依賴g2o的數值求導功能。
VertexPoseAndIntrinsics：代表一個相機的位姿和內參。這是一個9維的優化變量，其內部數據用一個自定義的
PoseAndIntrinsics結構體來存儲，該結構體使用Sophus::SO3d來處理旋轉，保證了其在李群流形上的正確性。該頂點類還定義了如何進行增量更新（oplusImpl）以及如何將3D點投影到圖像上（project）。VertexPoint：代表一個三維地圖點，是一個簡單的3維向量。
- 頂點 (Vertex)
：程序定義了兩類頂點：
- 邊 (Edge)
：程序定義了一類邊：
構建圖 (
SolveBA函數)：程序創建了一個g2o的
SparseOptimizer。遍歷所有相機和地圖點數據，創建相應的頂點實例，并設置其初始估計值，然后添加到優化器中。
特別地，它將所有地圖點頂點設置為待邊緣化 (
setMarginalized(true))。這是一個關鍵的性能優化步驟，它告訴g2o在求解線性系統時可以使用舒爾補技巧，先消去所有地圖點，求解相機位姿，再反向代入求解地圖點，這與Ceres的SPARSE_SCHUR求解器原理相同。遍歷所有觀測數據，創建
EdgeProjection實例，將其連接到對應的相機和地圖點頂點，并設置觀測值、信息矩陣和魯棒核函數，然后添加到優化器中。
求解與結果輸出：
程序配置求解器使用LM（Levenberg-Marquardt）算法和CSparse稀疏線性求解器。
調用
optimizer.optimize()啟動優化。優化完成后，從g2o的頂點中提取出優化后的相機參數和地圖點坐標，并寫回到
BALProblem對象中，以便保存到PLY文件進行可視化。
3.common.cpp
?BALProblem 類及其讀寫、歸一化與擾動流程
數據讀取與參數布局；
角軸?四元數轉換；
點云（PLY）導出；
基于中值和 MAD 的幾何歸一化；
對點與相機（旋轉、平移）添加高斯擾動。
該代碼定義并實現了一個名為?
BALProblem?的類，其核心功能是管理和處理用于大規模光束法平差 (Bundle Adjustment, BA) 的數據集。這個類是許多BA求解器示例（如Ceres和g2o）的數據處理前端，負責將原始數據文件加載到內存中，并提供一系列工具函數來操作這些數據。主要功能點如下：
數據加載與解析：
構造函數?
BALProblem::BALProblem?負責讀取一個特定格式（BAL格式）的文本文件。它解析文件內容，將相機參數（旋轉、平移、內參）、三維點坐標以及成千上萬的觀測數據（哪個相機看到了哪個點，以及2D像素坐標）加載到內存中的動態數組里。
它還支持在加載時將相機的旋轉表示從軸角 (Angle-Axis)?轉換為四元數 (Quaternion)，因為四元數在某些優化場景下表現更好（無奇異點）。
數據預處理與準備：
- 歸一化 (
Normalize)：實現了一個非常重要的預處理步驟。通過將整個場景的坐標系進行平移和縮放，使得3D點云的中心位于原點，并且點的離散程度（通過中位絕對偏差衡量）被縮放到一個固定的尺度（如100）。這可以極大地改善BA優化過程的數值穩定性和收斂速度。
- 添加擾動 (
Perturb)：為了模擬一個真實的、帶有誤差的初始估計，該函數可以對加載的（通常是真實的）相機位姿和3D點坐標添加指定標準差的高斯噪聲。這為后續的BA優化提供了一個有待改進的起點。
- 歸一化 (
數據格式轉換與訪問：
類內部封裝了相機參數在不同表示法之間的轉換邏輯，例如從優化器友好的?
(R, t)?表示法轉換為更直觀的相機中心?c?表示法（CameraToAngelAxisAndCenter）及其逆運算。提供了訪問內部數據（如相機、點、觀測）的接口函數（
mutable_cameras(),?mutable_points(), etc.）。
結果輸出與可視化：
WriteToFile?函數可以將內存中的（可能是優化后的）BA問題數據寫回到一個BAL格式的文件中。
WriteToPLYFile?函數可以將相機位姿（以相機中心表示）和3D點云導出為一個
.ply?文件。PLY是一種標準的三維模型文件格式，可以用MeshLab、CloudCompare等軟件打開，從而直觀地可視化優化前后的相機位姿和場景結構。
4.common.h?BALProblem 類的核心數據布局與接口
如何以一維數組管理 BA 問題的索引、觀測與參數，并提供歸一化、擾動及讀寫接口。
這個頭文件定義了一個名為?
BALProblem?的C++類，其核心功能是作為一個數據封裝和管理工具，專門用于處理和操作大規模光束法平差（Bundle Adjustment, BA）問題的數據。該類的設計目標是：
數據抽象與封裝：將從BAL數據文件中讀取的復雜數據（包括相機數量、點數量、觀測數量、大量的相機參數、3D點坐標和2D觀測值）封裝在一個單一的對象中。這使得上層應用（如BA求解器）可以方便地通過一個
BALProblem對象來訪問整個問題，而無需關心底層的文件解析和內存管理細節。內存管理：該類在構造時動態分配所需的大塊內存來存儲所有數據，并在析構時負責釋放這些內存，避免了內存泄漏。
數據訪問接口：它提供了一套全面、清晰的**接口函數（API）**來訪問數據。這些接口分為兩類：
- 只讀 (
const) 訪問：用于安全地獲取數據，如相機數量?
num_cameras()，或指向某個觀測對應參數的const指針?camera_for_observation()。 - 可修改 (
mutable) 訪問：提供可修改的指針，如
mutable_cameras()，允許優化器（如Ceres或g2o）直接在這些內存上修改參數值。
實用工具函數：該類還包含了一系列高級的、與BA問題緊密相關的工具函數：
WriteToFile?和?
WriteToPLYFile：用于結果的保存和可視化。Normalize：提供數據歸一化功能，這是改善大規模優化問題數值穩定性的關鍵步驟。
Perturb：用于給數據添加噪聲，以模擬真實的、非理想的初始狀態，從而可以測試優化算法的性能。
CameraToAngelAxisAndCenter?等私有函數：封裝了相機位姿不同表示法之間的轉換，簡化了類的內部實現。
靈活性：通過
use_quaternions參數，該類支持在內部使用軸角或四元數來表示旋轉，為不同的優化策略提供了靈活性。總之，
BALProblem類是一個精心設計的、功能完備的BA問題數據管理器，它極大地簡化了開發和測試大規模BA求解器的過程。這個頭文件本身不執行復雜的數學計算，但它定義了存儲和訪問各種數學實體的數據結構。
5.random.h
這個頭文件 (
random.h?或?rand.h) 提供了一組簡單而實用的隨機數生成工具函數。它的主要功能是：生成均勻分布隨機數 (
RandDouble)：
該函數利用C標準庫的?
rand()?函數生成一個偽隨機整數。然后通過歸一化操作，將其轉換為一個在?
[0.0, 1.0]?區間內均勻分布的?double?類型浮點數。這個函數是生成其他分布隨機數的基礎。
生成標準正態分布隨機數 (
RandNormal)：該函數實現了一種名為Marsaglia polar method?的算法。
該算法能夠從兩個獨立的均勻分布隨機數高效地生成兩個獨立的標準正態分布（也稱高斯分布，其均值為0，標準差/方差為1）的隨機數。
函數最終返回其中一個生成的正態分布隨機數。
這種隨機數在模擬現實世界中的測量噪聲、初始化算法等多種場景下都非常有用。
這兩個函數都被聲明為?
inline，這是一種性能優化建議，對于這樣短小的函數，編譯器很可能會采納這個建議，從而避免函數調用的開銷。注意：該實現依賴于?
rand()，它是一個比較基礎的偽隨機數生成器。為了獲得更高質量或可復現的隨機序列，現代C++（C++11及以后）推薦使用?<random>?庫中的工具，如?std::mt19937?和?std::uniform_real_distribution?/?std::normal_distribution。但對于許多簡單應用，此處的實現是足夠且方便的。
6.rotation.h
基本的點積與叉積計算；
角軸與四元數之間的穩定互轉公式及其零點近似；
使用 Rodrigues 公式（及其小角近似）對三維點執行旋轉變換。
這個頭文件 (
rotation.h) 提供了一組底層、高效且對自動求導友好的三維空間旋轉計算函數。這些函數都使用了C風格的數組指針作為輸入輸出，并被設計為模板函數，使其可以與double,?float，以及像Ceres Solver中的ceres::Jet這樣的特殊類型一起工作。其核心功能包括：
基本向量運算：
DotProduct：計算兩個三維向量的點積。
CrossProduct：計算兩個三維向量的叉積。
旋轉表示法之間的轉換：
AngleAxisToQuaternion：將軸角 (Angle-Axis)?表示的旋轉轉換為四元數 (Quaternion)。軸角表示為一個三維向量，其方向為旋轉軸，模長為旋轉角度。
QuaternionToAngleAxis：執行相反的轉換，將四元數轉換為軸角。
應用旋轉：
AngleAxisRotatePoint：使用軸角表示的旋轉來旋轉一個三維點。這個函數是核心，它內部實現了完整的羅德里格斯旋轉公式 (Rodrigues' Rotation Formula)。
數值穩定性與自動求導支持：
所有函數都特別處理了旋轉角度接近零的退化情況。在這種情況下，標準公式可能會因為除以一個接近零的數而變得數值不穩定。代碼通過使用泰勒級數展開來近似計算，這不僅解決了數值問題，還確保了當使用自動求導庫（如Ceres）時，能夠計算出正確且有意義的導數。
使用?
std::numeric_limits<double>::epsilon()?作為判斷是否接近零的閾值，這是一個健壯的做法。
這個頭文件是許多基于優化的三維計算機視覺應用（如BA）的基礎組件，因為它提供了將旋轉表示（如軸角）和其對點的作用（旋轉）進行計算的能力，并且這些計算對于優化求解器是“透明的”（即可以被自動求導）。
7.?SnavelyReprojectionError.h
在 Ceres 中實現帶徑向畸變的相機重投影誤差模型及其自動求導。
這個頭文件定義了一個名為?
SnavelyReprojectionError?的C++類，其唯一且核心的功能是為Ceres Solver框架定義一個代價函數（Cost Function），用于計算在Bundle Adjustment (BA)問題中的重投影誤差。該類的主要特點和功能如下：
封裝BA的代價函數：它將BA中一個核心的數學概念——重投影誤差——封裝成一個符合Ceres框架要求的C++類。這個誤差是指一個三維空間點根據當前的相機位姿和內參估計投影到圖像上時，其預測的2D位置與真實的2D觀測位置之間的差異。
實現Snavely相機模型：在靜態函數?
CamProjectionWithDistortion?中，它完整地實現了Snavely BAL數據集所使用的相機模型。這個模型包括：
用軸角（Angle-Axis）表示的旋轉。
三維平移。
一個簡化的內參模型，只包含焦距?f和兩個徑向畸變系數?k1,k2
。
支持Ceres自動求導：
通過將核心的?
operator()?定義為模板函數，該類能夠與Ceres的自動求導機制無縫協作。這意味著開發者無需手動計算復雜且容易出錯的雅可比矩陣。Ceres會在編譯時自動生成計算導數的代碼。
提供工廠方法 (
Create)：它提供了一個靜態的
Create方法，這是一個設計模式中的工廠方法。這個方法簡化了創建ceres::AutoDiffCostFunction實例的過程，將復雜的模板參數和new操作封裝起來，使得上層調用代碼更簡潔、更易讀。
總之，
SnavelyReprojectionError.h是Ceres BA示例中的一個關鍵組件，它精確地定義了優化問題的目標函數中的誤差項，并利用Ceres的特性，使得構建一個復雜的大規模優化問題變得相對簡單。
|rk356x驅動GPIO基礎應用之點亮led燈)
