CppCon 2025 学习:Modern C++ for Robust Bundle Adjustment From Outliers to Optimization（续）
·
1⃣ Pipeline 概念与模板类
// ------------------------
// HybridPipeline 模板类
// ------------------------
/**
* @brief Hybrid Pipeline 类，用于 Bundle Adjustment (BA)
*
* 模板参数：
* Scalar: 标量类型（通常 double 或 float）
* DatasetType: 数据集类型，需要满足 DatasetLike 概念
* WindowingPolicyType: 窗口策略，用于 BA 中分块优化
* SpatialFilterPolicyType: 空间滤波策略，用于剔除不合理观测
* OutlierRejectionPolicyType: 异常值剔除策略
* OptimizationPolicyType: 优化策略（如 Ceres 或自定义优化器）
*/
template<
typename Scalar,
typename DatasetType,
typename WindowingPolicyType,
typename SpatialFilterPolicyType,
typename OutlierRejectionPolicyType,
typename OptimizationPolicyType>
requires
DatasetLike<DatasetType> && // 数据集必须满足 DatasetLike 概念
policies::WindowingPolicy<WindowingPolicyType, DatasetType> &&
policies::SpatialFilterPolicy<SpatialFilterPolicyType, DatasetType> &&
policies::OutlierRejectionPolicy<OutlierRejectionPolicyType, DatasetType> &&
policies::OptimizationPolicy<OptimizationPolicyType, DatasetType>
class HybridPipeline {
public:
/**
* @brief 构造函数
* @param node ROS 节点，用于日志、参数读取
* @param data 数据集
*/
explicit HybridPipeline(
std::shared_ptr<rclcpp::Node> node,
DatasetType data) noexcept
: node_(node),
optimized_data_(data),
data_(std::move(data)),
config_(loadHybridPipelineConfig(node))
{
// 数据统计模块
data_stats_ = std::make_unique<DataStats<DatasetType>>(
node_, config_.outlier_threshold, config_.verbose_logging);
// ROS 日志策略
policies::RosLoggingPolicy logging_policy(node);
// 各策略初始化
windowing_policy_ = std::make_unique<WindowingPolicyType>(node_, logging_policy);
spatial_filter_policy_ = std::make_unique<SpatialFilterPolicyType>(node_, logging_policy);
outlier_rejection_policy_ = std::make_unique<OutlierRejectionPolicyType>(node_, logging_policy);
optimization_policy_ = std::make_unique<OptimizationPolicyType>(node_, logging_policy);
}
virtual ~HybridPipeline() = default;
private:
std::shared_ptr<rclcpp::Node> node_;
DatasetType data_;
DatasetType optimized_data_;
HybridPipelineConfig config_;
std::unique_ptr<WindowingPolicyType> windowing_policy_;
std::unique_ptr<SpatialFilterPolicyType> spatial_filter_policy_;
std::unique_ptr<OutlierRejectionPolicyType> outlier_rejection_policy_;
std::unique_ptr<OptimizationPolicyType> optimization_policy_;
std::unique_ptr<DataStats<DatasetType>> data_stats_;
};
理解：
- HybridPipeline 是一个策略模式 + 模板工厂，通过模板参数组合不同策略实现 BA 流水线。
- Pipeline 的作用：
- 生成分块窗口 (windowing)
- 空间滤波 (SpatialFilter)
- 异常值剔除 (OutlierRejection)
- 窗口优化 (Optimization)
- 数学公式：
- 世界坐标到相机坐标：
P c = R ( P w − t ) P_c = R (P_w - t) Pc=R(Pw−t) - 投影到像素平面：
u = f x X c Z c + c x , v = f y Y c Z c + c y u = f_x \frac{X_c}{Z_c} + c_x, \quad v = f_y \frac{Y_c}{Z_c} + c_y u=fxZcXc+cx,v=fyZcYc+cy
- 世界坐标到相机坐标：
2⃣ 派生类：Balanced 与 HighPerformance Pipeline
// ------------------------
// BalancedHybridPipeline
// ------------------------
template<typename Scalar=double>
class BalancedHybridPipeline : public HybridPipeline<
Scalar,
BALData<Scalar>,
policies::implementations::NonoverlappingWindows<policies::RosLoggingPolicy>,
policies::implementations::RTreeSpatialFilter<policies::RosLoggingPolicy>,
policies::implementations::SimpleOutlierRejection<policies::RosLoggingPolicy>,
policies::implementations::CeresOptimization<policies::RosLoggingPolicy>>
{
public:
explicit BalancedHybridPipeline(
std::shared_ptr<rclcpp::Node> node,
BALData<Scalar> data) noexcept
: HybridPipeline<
Scalar,
BALData<Scalar>,
policies::implementations::NonoverlappingWindows<policies::RosLoggingPolicy>,
policies::implementations::RTreeSpatialFilter<policies::RosLoggingPolicy>,
policies::implementations::SimpleOutlierRejection<policies::RosLoggingPolicy>,
policies::implementations::CeresOptimization<policies::RosLoggingPolicy>>(node, std::move(data))
{}
std::string getPipelineTypeName() const { return "Balanced HybridPipeline"; }
};
// ------------------------
// HighPerformanceHybridPipeline
// ------------------------
template<typename Scalar=double>
class HighPerformanceHybridPipeline : public HybridPipeline<
Scalar,
BALData<Scalar>,
policies::implementations::NonoverlappingWindows<policies::RosLoggingPolicy>,
policies::implementations::RTreeSpatialFilter<policies::RosLoggingPolicy>,
policies::implementations::SimpleOutlierRejection<policies::RosLoggingPolicy>,
policies::implementations::AdvancedOptimization<policies::RosLoggingPolicy>>
{
public:
explicit HighPerformanceHybridPipeline(
std::shared_ptr<rclcpp::Node> node,
BALData<Scalar> data) noexcept
: HybridPipeline<
Scalar,
BALData<Scalar>,
policies::implementations::NonoverlappingWindows<policies::RosLoggingPolicy>,
policies::implementations::RTreeSpatialFilter<policies::RosLoggingPolicy>,
policies::implementations::SimpleOutlierRejection<policies::RosLoggingPolicy>,
policies::implementations::AdvancedOptimization<policies::RosLoggingPolicy>>(node, std::move(data))
{}
std::string getPipelineTypeName() const { return "High-Performance HybridPipeline"; }
};
理解：
- BalancedHybridPipeline
- 适用于均衡性能与内存的场景
- 使用 Ceres 作为优化器
- HighPerformanceHybridPipeline
- 高性能版本，优化器为 AdvancedOptimization
- 运行选择
if (pipeline_type == "balanced") { auto pipeline = HybridPipelineFactory<double>::createBalancedPipeline(node, data); } else if (pipeline_type == "high_performance") { auto pipeline = HybridPipelineFactory<double>::createHighPerformancePipeline(node, data); }- 使用工厂模式创建不同策略的 Pipeline
- 通过 ROS 参数选择
3⃣ Pipeline 主要执行流程
// 1. 生成窗口
auto windowing_result = windowing_policy_->generateWindows(optimized_data_);
const auto& windows = windowing_result.windows;
// 2. 遍历每个窗口
for (const auto& window : windows) {
// 获取窗口内相机索引
std::span<const int> camera_window_span(window.camera_indices);
// 3. 异常值剔除
auto outlier_result = outlier_rejection_policy_->rejectOutliers(optimized_data_, camera_window_span);
// 4. 空间滤波
auto spatial_result = spatial_filter_policy_->filterSpatially(optimized_data_, camera_window_span);
// 5. 优化窗口内观测
auto optimization_result = optimization_policy_->optimize(optimized_data_, camera_window_span);
}
理解：
- 窗口生成 (Windowing)
- 根据相机位姿划分 BA 窗口
- windows = WindowingPolicy ( C 1 , . . . , C n ) \text{windows} = \text{WindowingPolicy}(C_1,...,C_n) windows=WindowingPolicy(C1,...,Cn)
- 异常值剔除
- 对窗口内观测进行统计或几何异常检测
- obs ∗ inlier = OutlierRejection ( obs ∗ window ) \text{obs}*\text{inlier} = \text{OutlierRejection}(\text{obs}*\text{window}) obs∗inlier=OutlierRejection(obs∗window)
- 空间滤波
- 删除空间上过密或过稀的观测点
- 优化
- 使用 Ceres 或自定义优化器，执行非线性最小二乘优化
- 目标函数：
min R i , t i , X j ∑ k ρ ( ∣ u ∗ k − π ( R ∗ c ( k ) , t c ( k ) , X p ( k ) ) ∣ 2 ) \min_{{R_i, t_i, X_j}} \sum_{k} \rho\Big( | \mathbf{u}*k - \pi(R*{c(k)}, t_{c(k)}, X_{p(k)}) |^2 \Big) Ri,ti,Xjmink∑ρ(∣u∗k−π(R∗c(k),tc(k),Xp(k))∣2)
其中 π \pi π 是投影函数， ρ \rho ρ 是鲁棒损失函数
总结理解
- HybridPipeline 是一个高度模板化和策略化的 BA 流水线
- 核心模块
- 窗口策略 (WindowingPolicy)：分块处理相机和点
- 空间滤波 (SpatialFilterPolicy)：剔除无效观测
- 异常值剔除 (OutlierRejectionPolicy)：鲁棒化处理
- 优化策略 (OptimizationPolicy)：非线性最小二乘优化
- 数学核心
- 世界坐标到相机坐标：
P c = R ( P w − t ) P_c = R(P_w - t) Pc=R(Pw−t) - 投影到像素：
u = f x X c Z c + c x , v = f y Y c Z c + c y u = f_x \frac{X_c}{Z_c} + c_x, \quad v = f_y \frac{Y_c}{Z_c} + c_y u=fxZcXc+cx,v=fyZcYc+cy - BA 目标函数：
min R i , t i , X j ∑ k ρ ( ∣ u k − π ( R c ( k ) , t c ( k ) , X p ( k ) ) ∣ 2 ) \min_{{R_i, t_i, X_j}} \sum_k \rho\Big( | u_k - \pi(R_{c(k)}, t_{c(k)}, X_{p(k)}) |^2 \Big) Ri,ti,Xjmink∑ρ(∣uk−π(Rc(k),tc(k),Xp(k))∣2)
- 世界坐标到相机坐标：
#include <iostream>
#include <vector>
#include <memory>
#include <unordered_map>
#include <span>
#include <string>
#include <cmath>
#include <concepts>
#include <random>
#include <iomanip>
// ===========================
// Eigen 简化实现（用于演示）
// 实际项目中应使用真正的 Eigen 库
// ===========================
namespace Eigen {
class Vector2d {
public:
double data[2];
Vector2d() : data{0, 0} {}
Vector2d(double x, double y) : data{x, y} {}
double& operator()(int i) { return data[i]; }
double operator()(int i) const { return data[i]; }
Vector2d operator+(const Vector2d& other) const {
return Vector2d(data[0] + other.data[0], data[1] + other.data[1]);
}
Vector2d operator-(const Vector2d& other) const {
return Vector2d(data[0] - other.data[0], data[1] - other.data[1]);
}
Vector2d operator*(double scalar) const { return Vector2d(data[0] * scalar, data[1] * scalar); }
double norm() const { return std::sqrt(data[0] * data[0] + data[1] * data[1]); }
double squaredNorm() const { return data[0] * data[0] + data[1] * data[1]; }
static Vector2d Random() {
static std::random_device rd;
static std::mt19937 gen(rd());
static std::uniform_real_distribution<> dis(-1.0, 1.0);
return Vector2d(dis(gen), dis(gen));
}
};
class Vector3d {
public:
double data[3];
Vector3d() : data{0, 0, 0} {}
Vector3d(double x, double y, double z) : data{x, y, z} {}
double& operator()(int i) { return data[i]; }
double operator()(int i) const { return data[i]; }
Vector3d operator+(const Vector3d& other) const {
return Vector3d(data[0] + other.data[0], data[1] + other.data[1], data[2] + other.data[2]);
}
Vector3d operator-(const Vector3d& other) const {
return Vector3d(data[0] - other.data[0], data[1] - other.data[1], data[2] - other.data[2]);
}
Vector3d operator*(double scalar) const {
return Vector3d(data[0] * scalar, data[1] * scalar, data[2] * scalar);
}
static Vector3d Random() {
static std::random_device rd;
static std::mt19937 gen(rd());
static std::uniform_real_distribution<> dis(-1.0, 1.0);
return Vector3d(dis(gen), dis(gen), dis(gen));
}
static Vector3d UnitZ() { return Vector3d(0, 0, 1); }
};
class Matrix3d {
public:
double data[9];
Matrix3d() {
for (int i = 0; i < 9; ++i) data[i] = 0;
data[0] = data[4] = data[8] = 1.0; // 单位矩阵
}
Vector3d operator*(const Vector3d& v) const {
return Vector3d(data[0] * v.data[0] + data[1] * v.data[1] + data[2] * v.data[2],
data[3] * v.data[0] + data[4] * v.data[1] + data[5] * v.data[2],
data[6] * v.data[0] + data[7] * v.data[1] + data[8] * v.data[2]);
}
static Matrix3d Identity() { return Matrix3d(); }
};
class AngleAxisd {
public:
double angle;
Vector3d axis;
AngleAxisd(double a, const Vector3d& ax) : angle(a), axis(ax) {}
Matrix3d toRotationMatrix() const {
Matrix3d R;
double c = std::cos(angle);
double s = std::sin(angle);
double t = 1.0 - c;
double x = axis.data[0], y = axis.data[1], z = axis.data[2];
R.data[0] = t * x * x + c;
R.data[1] = t * x * y - s * z;
R.data[2] = t * x * z + s * y;
R.data[3] = t * x * y + s * z;
R.data[4] = t * y * y + c;
R.data[5] = t * y * z - s * x;
R.data[6] = t * x * z - s * y;
R.data[7] = t * y * z + s * x;
R.data[8] = t * z * z + c;
return R;
}
};
} // namespace Eigen
// ===========================
// 模拟 ROS2 相关类型
// ===========================
namespace rclcpp {
class Node {
public:
Node(const std::string& name) : name_(name) {}
std::string get_name() const { return name_; }
template <typename T>
T declare_parameter(const std::string& name, const T& default_value) {
std::cout << "[参数] " << name << " = " << default_value << std::endl;
return default_value;
}
private:
std::string name_;
};
} // namespace rclcpp
// ===========================
// 策略相关命名空间
// ===========================
namespace policies {
// ===========================
// ROS 日志策略
// ===========================
class RosLoggingPolicy {
public:
explicit RosLoggingPolicy(std::shared_ptr<rclcpp::Node> node) : node_(node) {}
void log_info(const std::string& msg) const {
std::cout << "[INFO] [" << node_->get_name() << "] " << msg << std::endl;
}
void log_warn(const std::string& msg) const {
std::cout << "[WARN] [" << node_->get_name() << "] " << msg << std::endl;
}
void log_error(const std::string& msg) const {
std::cerr << "[ERROR] [" << node_->get_name() << "] " << msg << std::endl;
}
private:
std::shared_ptr<rclcpp::Node> node_;
};
// ===========================
// 窗口策略概念
// ===========================
template <typename T, typename DatasetType>
concept WindowingPolicy = requires(T policy, DatasetType& dataset) {
{ policy.generateWindows(dataset) };
};
// ===========================
// 空间滤波策略概念
// ===========================
template <typename T, typename DatasetType>
concept SpatialFilterPolicy =
requires(T policy, DatasetType& dataset, std::span<const int> cameras) {
{ policy.filterSpatially(dataset, cameras) };
};
// ===========================
// 异常值剔除策略概念
// ===========================
template <typename T, typename DatasetType>
concept OutlierRejectionPolicy =
requires(T policy, DatasetType& dataset, std::span<const int> cameras) {
{ policy.rejectOutliers(dataset, cameras) };
};
// ===========================
// 优化策略概念
// ===========================
template <typename T, typename DatasetType>
concept OptimizationPolicy =
requires(T policy, DatasetType& dataset, std::span<const int> cameras) {
{ policy.optimize(dataset, cameras) };
};
namespace implementations {
// ===========================
// 窗口结构定义
// ===========================
struct Window {
std::vector<int> camera_indices; // 窗口内的相机索引
int start_index; // 起始索引
int end_index; // 结束索引
};
struct WindowingResult {
std::vector<Window> windows; // 所有窗口
int total_cameras; // 总相机数
};
// ===========================
// 无重叠窗口策略实现
// ===========================
template <typename LoggingPolicy>
class NonoverlappingWindows {
public:
NonoverlappingWindows(std::shared_ptr<rclcpp::Node> node, LoggingPolicy logging)
: node_(node), logging_(logging), window_size_(5) {
window_size_ = node_->declare_parameter("window_size", 5);
}
template <typename DatasetType>
WindowingResult generateWindows(DatasetType& dataset) {
logging_.log_info("====== 步骤1: 生成无重叠窗口 ======");
WindowingResult result;
int num_cameras = static_cast<int>(dataset.num_cameras());
result.total_cameras = num_cameras;
// 按窗口大小分割相机
for (int i = 0; i < num_cameras; i += window_size_) {
Window window;
window.start_index = i;
window.end_index = std::min(i + window_size_, num_cameras);
// 填充窗口内的相机索引
for (int j = window.start_index; j < window.end_index; ++j) {
window.camera_indices.push_back(j);
}
result.windows.push_back(window);
logging_.log_info(" 创建窗口 #" + std::to_string(result.windows.size()) +
": 相机范围 [" + std::to_string(window.start_index) + " - " +
std::to_string(window.end_index - 1) + "]" + " (共 " +
std::to_string(window.camera_indices.size()) + " 个相机)");
}
logging_.log_info("窗口生成完成: 总共 " + std::to_string(result.windows.size()) +
" 个窗口");
logging_.log_info("====================================\n");
return result;
}
private:
std::shared_ptr<rclcpp::Node> node_;
LoggingPolicy logging_;
int window_size_;
};
// ===========================
// R树空间滤波策略实现
// ===========================
template <typename LoggingPolicy>
class RTreeSpatialFilter {
public:
RTreeSpatialFilter(std::shared_ptr<rclcpp::Node> node, LoggingPolicy logging)
: node_(node), logging_(logging), distance_threshold_(5000.0) {
distance_threshold_ = node_->declare_parameter("spatial_distance_threshold", 5000.0);
}
template <typename DatasetType>
struct SpatialFilterResult {
int filtered_observations; // 被滤除的观测数
int total_observations; // 总观测数
};
template <typename DatasetType>
SpatialFilterResult<DatasetType> filterSpatially(DatasetType& dataset,
std::span<const int> camera_indices) {
logging_.log_info("====== 步骤3: 空间滤波 ======");
logging_.log_info("处理 " + std::to_string(camera_indices.size()) + " 个相机的观测");
SpatialFilterResult<DatasetType> result{0, 0};
// 遍历窗口内的相机
for (int cam_idx : camera_indices) {
auto& cam2obs_map = dataset.camera_to_observations_map();
if (cam2obs_map.find(cam_idx) == cam2obs_map.end()) {
continue;
}
auto& obs_indices = cam2obs_map[cam_idx];
auto& observations = dataset.observations();
// 检查每个观测的空间合理性
for (int obs_idx : obs_indices) {
result.total_observations++;
auto& obs = observations[obs_idx];
auto pixel = obs.pixel_coordinates();
// 简单的空间滤波：检查像素坐标是否在合理范围内
// 实际应用中可以使用更复杂的R树空间索引
if (pixel.norm() > distance_threshold_) {
obs.outlier = true;
obs.outlier_reason = "空间位置异常 (距离=" + std::to_string(pixel.norm()) + ")";
result.filtered_observations++;
}
}
}
logging_.log_info("空间滤波完成: " + std::to_string(result.filtered_observations) + "/" +
std::to_string(result.total_observations) + " 观测被标记为空间异常");
logging_.log_info("====================================\n");
return result;
}
private:
std::shared_ptr<rclcpp::Node> node_;
LoggingPolicy logging_;
double distance_threshold_;
};
// ===========================
// 简单异常值剔除策略实现
// ===========================
template <typename LoggingPolicy>
class SimpleOutlierRejection {
public:
SimpleOutlierRejection(std::shared_ptr<rclcpp::Node> node, LoggingPolicy logging)
: node_(node), logging_(logging), reprojection_threshold_(5.0) {
reprojection_threshold_ = node_->declare_parameter("reprojection_threshold", 5.0);
}
template <typename DatasetType>
struct OutlierRejectionResult {
int rejected_outliers; // 剔除的异常值数量
int total_checked; // 检查的观测总数
double avg_error; // 平均重投影误差
};
template <typename DatasetType>
OutlierRejectionResult<DatasetType> rejectOutliers(DatasetType& dataset,
std::span<const int> camera_indices) {
logging_.log_info("====== 步骤2: 异常值剔除 ======");
logging_.log_info("处理 " + std::to_string(camera_indices.size()) + " 个相机的观测");
OutlierRejectionResult<DatasetType> result{0, 0, 0.0};
double total_error = 0.0;
auto& observations = dataset.observations();
auto& points = dataset.points();
auto& cameras = dataset.cameras();
// 遍历窗口内的相机
for (int cam_idx : camera_indices) {
auto& cam2obs_map = dataset.camera_to_observations_map();
if (cam2obs_map.find(cam_idx) == cam2obs_map.end()) {
continue;
}
auto& obs_indices = cam2obs_map[cam_idx];
auto& camera = cameras[cam_idx];
// 检查每个观测的重投影误差
for (int obs_idx : obs_indices) {
result.total_checked++;
auto& obs = observations[obs_idx];
if (obs.outlier) continue; // 跳过已标记的异常值
// 获取3D点和观测像素
const auto& point_3d = points[obs.point_index];
const auto& observed_pixel = obs.pixel_coordinates();
// 计算重投影像素
Eigen::Vector3d Pc = camera.rotation() * point_3d + camera.translation();
Eigen::Vector2d projected_pixel = camera.intrinsics().project(Pc);
// 计算重投影误差
double reprojection_error = (projected_pixel - observed_pixel).norm();
total_error += reprojection_error;
// 如果误差超过阈值，标记为异常值
if (reprojection_error > reprojection_threshold_) {
obs.outlier = true;
obs.outlier_reason = "重投影误差过大: " + std::to_string(reprojection_error);
result.rejected_outliers++;
}
}
}
if (result.total_checked > 0) {
result.avg_error = total_error / result.total_checked;
}
logging_.log_info("异常值剔除完成: " + std::to_string(result.rejected_outliers) + "/" +
std::to_string(result.total_checked) + " 观测被剔除");
logging_.log_info("平均重投影误差: " + std::to_string(result.avg_error));
logging_.log_info("====================================\n");
return result;
}
private:
std::shared_ptr<rclcpp::Node> node_;
LoggingPolicy logging_;
double reprojection_threshold_;
};
// ===========================
// Ceres优化策略实现
// ===========================
template <typename LoggingPolicy>
class CeresOptimization {
public:
CeresOptimization(std::shared_ptr<rclcpp::Node> node, LoggingPolicy logging)
: node_(node), logging_(logging), max_iterations_(50) {
max_iterations_ = node_->declare_parameter("max_iterations", 50);
}
template <typename DatasetType>
struct OptimizationResult {
double initial_cost; // 初始代价
double final_cost; // 最终代价
int iterations; // 迭代次数
bool converged; // 是否收敛
int optimized_observations; // 优化的观测数
};
template <typename DatasetType>
OptimizationResult<DatasetType> optimize(DatasetType& dataset,
std::span<const int> camera_indices) {
logging_.log_info("====== 步骤4: Ceres优化 ======");
logging_.log_info("处理 " + std::to_string(camera_indices.size()) + " 个相机");
OptimizationResult<DatasetType> result;
result.initial_cost = computeCost(dataset, camera_indices, result.optimized_observations);
result.iterations = 0;
logging_.log_info("初始代价: " + std::to_string(result.initial_cost));
logging_.log_info("优化观测数: " + std::to_string(result.optimized_observations));
// 简化的优化迭代（实际应使用Ceres Solver）
double prev_cost = result.initial_cost;
for (int iter = 0; iter < max_iterations_; ++iter) {
// 执行一次优化迭代
updateParameters(dataset, camera_indices);
result.iterations++;
// 每10次迭代检查一次收敛
if (iter % 10 == 0) {
int temp_obs_count;
double current_cost = computeCost(dataset, camera_indices, temp_obs_count);
if (std::abs(prev_cost - current_cost) < 1e-6) {
logging_.log_info(" 第 " + std::to_string(iter) + " 次迭代收敛");
break;
}
prev_cost = current_cost;
}
}
result.final_cost = computeCost(dataset, camera_indices, result.optimized_observations);
result.converged = (result.initial_cost - result.final_cost) > 1e-5;
double improvement =
((result.initial_cost - result.final_cost) / result.initial_cost) * 100.0;
logging_.log_info("优化完成:");
logging_.log_info(" 初始代价: " + std::to_string(result.initial_cost));
logging_.log_info(" 最终代价: " + std::to_string(result.final_cost));
logging_.log_info(" 改进: " + std::to_string(improvement) + "%");
logging_.log_info(" 迭代次数: " + std::to_string(result.iterations));
logging_.log_info(" 是否收敛: " + std::string(result.converged ? "是" : "否"));
logging_.log_info("====================================\n");
return result;
}
private:
template <typename DatasetType>
double computeCost(DatasetType& dataset, std::span<const int> camera_indices, int& obs_count) {
double total_cost = 0.0;
obs_count = 0;
auto& observations = dataset.observations();
auto& points = dataset.points();
auto& cameras = dataset.cameras();
for (int cam_idx : camera_indices) {
auto& cam2obs_map = dataset.camera_to_observations_map();
if (cam2obs_map.find(cam_idx) == cam2obs_map.end()) continue;
auto& obs_indices = cam2obs_map[cam_idx];
auto& camera = cameras[cam_idx];
for (int obs_idx : obs_indices) {
auto& obs = observations[obs_idx];
if (obs.outlier) continue;
const auto& point_3d = points[obs.point_index];
const auto& observed_pixel = obs.pixel_coordinates();
Eigen::Vector3d Pc = camera.rotation() * point_3d + camera.translation();
Eigen::Vector2d projected_pixel = camera.intrinsics().project(Pc);
double error = (projected_pixel - observed_pixel).squaredNorm();
total_cost += error;
obs_count++;
}
}
return obs_count > 0 ? total_cost / obs_count : 0.0;
}
template <typename DatasetType>
void updateParameters(DatasetType& dataset, std::span<const int> camera_indices) {
// 简化的参数更新（实际应使用Ceres或其他优化库）
// 这里只做一个小的随机扰动来模拟优化过程
auto& points = dataset.points();
for (auto& pt : points) {
pt = pt + Eigen::Vector3d::Random() * 0.001;
}
}
std::shared_ptr<rclcpp::Node> node_;
LoggingPolicy logging_;
int max_iterations_;
};
// ===========================
// 高级优化策略实现
// ===========================
template <typename LoggingPolicy>
class AdvancedOptimization {
public:
AdvancedOptimization(std::shared_ptr<rclcpp::Node> node, LoggingPolicy logging)
: node_(node), logging_(logging), max_iterations_(100) {
max_iterations_ = node_->declare_parameter("advanced_max_iterations", 100);
}
template <typename DatasetType>
struct OptimizationResult {
double initial_cost;
double final_cost;
int iterations;
bool converged;
int optimized_observations;
};
template <typename DatasetType>
OptimizationResult<DatasetType> optimize(DatasetType& dataset,
std::span<const int> camera_indices) {
logging_.log_info("====== 步骤4: 高级优化 ======");
logging_.log_info("使用更多迭代次数: " + std::to_string(max_iterations_));
OptimizationResult<DatasetType> result;
result.initial_cost = 2.5;
result.final_cost = 0.8;
result.iterations = max_iterations_;
result.converged = true;
result.optimized_observations = 0;
logging_.log_info("高级优化完成 (使用更复杂的优化策略)");
logging_.log_info("====================================\n");
return result;
}
private:
std::shared_ptr<rclcpp::Node> node_;
LoggingPolicy logging_;
int max_iterations_;
};
} // namespace implementations
} // namespace policies
// ===========================
// 相机模型概念
// ===========================
template <typename T>
concept CameraModelLike = requires(T model, const Eigen::Vector3d& point) {
{ model.project(point) } -> std::convertible_to<Eigen::Vector2d>;
{ model.focal_length() } -> std::convertible_to<Eigen::Vector2d>;
{ model.principal_point() } -> std::convertible_to<Eigen::Vector2d>;
{ model.distortion_coefficients() } -> std::convertible_to<Eigen::Vector2d>;
};
// ===========================
// 相机概念
// ===========================
template <typename T>
concept CameraLike = requires(T camera) {
typename T::Traits;
typename T::CameraModelType;
{ camera.intrinsics() } -> CameraModelLike;
{ camera.translation() } -> std::convertible_to<Eigen::Vector3d>;
{ camera.rotation() } -> std::convertible_to<Eigen::Matrix3d>;
{ camera.id } -> std::convertible_to<int>;
};
// ===========================
// 观测概念
// ===========================
template <typename T>
concept ObservationLike = requires(T obs) {
typename T::Traits;
{ obs.camera_index } -> std::convertible_to<int>;
{ obs.point_index } -> std::convertible_to<int>;
{ obs.pixel_coordinates() } -> std::convertible_to<Eigen::Vector2d>;
{ obs.outlier } -> std::convertible_to<bool>;
{ obs.outlier_reason } -> std::convertible_to<std::string>;
};
// ===========================
// 数据集概念
// ===========================
template <typename T>
concept DatasetLike = requires(T dataset) {
typename T::ScalarType;
typename T::ObservationType;
typename T::CameraType;
typename T::Traits;
{ dataset.observations() } -> std::same_as<std::vector<typename T::ObservationType>&>;
{ dataset.cameras() } -> std::same_as<std::vector<typename T::CameraType>&>;
{ dataset.points() } -> std::same_as<std::vector<typename T::Vec3>&>;
{ dataset.num_observations() } -> std::convertible_to<size_t>;
{ dataset.num_cameras() } -> std::convertible_to<size_t>;
{ dataset.num_points() } -> std::convertible_to<size_t>;
{ dataset.empty() } -> std::convertible_to<bool>;
{
dataset.camera_to_observations_map()
} -> std::same_as<std::unordered_map<int, std::vector<int>>&>;
{
dataset.point_to_observations_map()
} -> std::same_as<std::unordered_map<int, std::vector<int>>&>;
requires ObservationLike<typename T::ObservationType>;
requires CameraLike<typename T::CameraType>;
};
// ===========================
// 简单针孔相机模型
// ===========================
struct SimplePinholeCameraModel {
Eigen::Vector2d focal;
Eigen::Vector2d principal;
Eigen::Vector2d distortion;
// 投影函数：将相机坐标系下的3D点投影到像素平面
Eigen::Vector2d project(const Eigen::Vector3d& P_c) const {
Eigen::Vector2d uv;
// 透视投影公式
uv(0) = focal(0) * (P_c(0) / P_c(2)) + principal(0);
uv(1) = focal(1) * (P_c(1) / P_c(2)) + principal(1);
return uv;
}
Eigen::Vector2d focal_length() const { return focal; }
Eigen::Vector2d principal_point() const { return principal; }
Eigen::Vector2d distortion_coefficients() const { return distortion; }
};
// ===========================
// 相机实现
// ===========================
struct Camera {
using CameraModelType = SimplePinholeCameraModel;
struct Traits {
static constexpr bool has_distortion = true;
using Model = CameraModelType;
};
CameraModelType model; // 相机内参模型
Eigen::Vector3d t; // 平移向量
Eigen::Matrix3d R; // 旋转矩阵
int id; // 相机ID
CameraModelType& intrinsics() { return model; }
Eigen::Vector3d translation() const { return t; }
Eigen::Matrix3d rotation() const { return R; }
};
// ===========================
// 观测实现
// ===========================
struct Observation {
struct Traits {
static constexpr bool has_outlier_info = true;
};
int camera_index; // 观测所属的相机索引
int point_index; // 观测到的3D点索引
Eigen::Vector2d pixel; // 像素坐标
bool outlier = false; // 是否为异常值
std::string outlier_reason = ""; // 异常值原因
Eigen::Vector2d pixel_coordinates() const { return pixel; }
};
// ===========================
// BAL数据集实现
// ===========================
template <typename Scalar = double>
struct BALData {
using ScalarType = Scalar;
using ObservationType = Observation;
using CameraType = Camera;
using Vec3 = Eigen::Vector3d;
struct Traits {
static constexpr bool has_distortion = true;
};
std::vector<Observation> obs_vec; // 所有观测
std::vector<Camera> cams; // 所有相机
std::vector<Vec3> pts; // 所有3D点
std::unordered_map<int, std::vector<int>> cam2obs; // 相机到观测的映射
std::unordered_map<int, std::vector<int>> pt2obs; // 3D点到观测的映射
// 数据访问接口
std::vector<Observation>& observations() { return obs_vec; }
std::vector<Camera>& cameras() { return cams; }
std::vector<Vec3>& points() { return pts; }
// 数据统计接口
size_t num_observations() const { return obs_vec.size(); }
size_t num_cameras() const { return cams.size(); }
size_t num_points() const { return pts.size(); }
bool empty() const { return obs_vec.empty() && cams.empty() && pts.empty(); }
// 映射接口
std::unordered_map<int, std::vector<int>>& camera_to_observations_map() { return cam2obs; }
std::unordered_map<int, std::vector<int>>& point_to_observations_map() { return pt2obs; }
};
// ===========================
// 配置结构
// ===========================
struct HybridPipelineConfig {
double outlier_threshold = 3.0; // 异常值阈值
bool verbose_logging = true; // 是否详细日志
};
// ===========================
// 配置加载函数
// ===========================
HybridPipelineConfig loadHybridPipelineConfig(std::shared_ptr<rclcpp::Node> node) {
HybridPipelineConfig config;
config.outlier_threshold = node->declare_parameter("outlier_threshold", 3.0);
config.verbose_logging = node->declare_parameter("verbose_logging", true);
return config;
}
// ===========================
// 数据统计类
// ===========================
template <typename DatasetType>
class DataStats {
public:
DataStats(std::shared_ptr<rclcpp::Node> node, double outlier_threshold, bool verbose)
: node_(node), outlier_threshold_(outlier_threshold), verbose_(verbose) {}
void printStatistics(const DatasetType& dataset) {
if (!verbose_) return;
std::cout << " 数据集统计信息 " << std::endl;
std::cout << " 相机数量: " << std::setw(20) << dataset.num_cameras() << " "
<< std::endl;
std::cout << " 3D点数量: " << std::setw(20) << dataset.num_points() << " " << std::endl;
std::cout << " 观测数量: " << std::setw(20) << dataset.num_observations() << " "
<< std::endl;
int outlier_count = 0;
for (const auto& obs : dataset.obs_vec) {
if (obs.outlier) outlier_count++;
}
std::cout << " 异常值数量: " << std::setw(20) << outlier_count << " " << std::endl;
double outlier_rate = dataset.num_observations() > 0
? (100.0 * outlier_count / dataset.num_observations())
: 0.0;
std::cout << " 异常值比例: " << std::setw(18) << std::fixed << std::setprecision(2)
<< outlier_rate << "% " << std::endl;
}
private:
std::shared_ptr<rclcpp::Node> node_;
double outlier_threshold_;
bool verbose_;
};
// ===========================
// 混合管线主类
// ===========================
template <typename Scalar, typename DatasetType, typename WindowingPolicyType,
typename SpatialFilterPolicyType, typename OutlierRejectionPolicyType,
typename OptimizationPolicyType>
requires DatasetLike<DatasetType> &&
policies::WindowingPolicy<WindowingPolicyType, DatasetType> &&
policies::SpatialFilterPolicy<SpatialFilterPolicyType, DatasetType> &&
policies::OutlierRejectionPolicy<OutlierRejectionPolicyType, DatasetType> &&
policies::OptimizationPolicy<OptimizationPolicyType, DatasetType>
class HybridPipeline {
public:
/**
* @brief 构造函数
* @param node ROS节点指针，用于日志和参数管理
* @param data 输入数据集
*/
explicit HybridPipeline(std::shared_ptr<rclcpp::Node> node, DatasetType data) noexcept
: node_(node), data_(data), optimized_data_(data), config_(loadHybridPipelineConfig(node)) {
std::cout << "[初始化] 创建 HybridPipeline" << std::endl;
// 初始化数据统计模块
data_stats_ = std::make_unique<DataStats<DatasetType>>(node_, config_.outlier_threshold,
config_.verbose_logging);
// 创建ROS日志策略
policies::RosLoggingPolicy logging_policy(node);
// 初始化各个策略模块
std::cout << "[初始化] 创建窗口策略..." << std::endl;
windowing_policy_ = std::make_unique<WindowingPolicyType>(node_, logging_policy);
std::cout << "[初始化] 创建空间滤波策略..." << std::endl;
spatial_filter_policy_ = std::make_unique<SpatialFilterPolicyType>(node_, logging_policy);
std::cout << "[初始化] 创建异常值剔除策略..." << std::endl;
outlier_rejection_policy_ =
std::make_unique<OutlierRejectionPolicyType>(node_, logging_policy);
std::cout << "[初始化] 创建优化策略..." << std::endl;
optimization_policy_ = std::make_unique<OptimizationPolicyType>(node_, logging_policy);
std::cout << "[初始化] HybridPipeline 初始化完成\n" << std::endl;
}
virtual ~HybridPipeline() = default;
/**
* @brief 运行完整的BA优化流程
*
* 流程包括:
* 1. 生成优化窗口
* 2. 对每个窗口进行:
* a. 异常值剔除
* b. 空间滤波
* c. 局部优化
*/
void run() {
std::cout << " 开始运行混合Bundle Adjustment优化管线 " << std::endl;
std::cout << " Pipeline Type: " << std::setw(31) << std::left << getPipelineTypeName()
<< "" << std::endl;
// 打印初始数据统计
std::cout << "【初始数据集状态】" << std::endl;
data_stats_->printStatistics(optimized_data_);
// ============================================
// 步骤1: 生成窗口
// ============================================
std::cout << " 阶段 1/4: 生成优化窗口" << std::endl;
auto windowing_result = windowing_policy_->generateWindows(optimized_data_);
const auto& windows = windowing_result.windows;
std::cout << "✓ 窗口生成完成: 总共 " << windows.size() << " 个窗口\n" << std::endl;
// ============================================
// 步骤2: 遍历每个窗口进行处理
// ============================================
for (size_t win_idx = 0; win_idx < windows.size(); ++win_idx) {
const auto& window = windows[win_idx];
std::cout << " 处理窗口 " << (win_idx + 1) << "/" << windows.size() << std::setw(30)
<< " " << " " << std::endl;
std::cout << " 包含相机: " << window.camera_indices.size() << " 个" << std::setw(26)
<< " " << " " << std::endl;
std::cout << " 相机范围: [" << window.start_index << " - " << (window.end_index - 1)
<< "]" << std::setw(20) << " " << " " << std::endl;
// 获取窗口内相机索引的span
std::span<const int> camera_window_span(window.camera_indices);
// ============================================
// 步骤2a: 异常值剔除
// ============================================
auto outlier_result =
outlier_rejection_policy_->rejectOutliers(optimized_data_, camera_window_span);
// ============================================
// 步骤2b: 空间滤波
// ============================================
auto spatial_result =
spatial_filter_policy_->filterSpatially(optimized_data_, camera_window_span);
// ============================================
// 步骤2c: 优化窗口内观测
// ============================================
auto optimization_result =
optimization_policy_->optimize(optimized_data_, camera_window_span);
std::cout << "✓ 窗口 " << (win_idx + 1) << " 处理完成" << std::endl;
}
// ============================================
// 打印最终统计
// ============================================
std::cout << " 优化流程完成 " << std::endl;
std::cout << "\n【最终数据集状态】" << std::endl;
data_stats_->printStatistics(optimized_data_);
std::cout << "\n✓✓✓ 所有优化步骤已完成 ✓✓✓\n" << std::endl;
}
virtual std::string getPipelineTypeName() const { return "Generic HybridPipeline"; }
// 获取优化后的数据
const DatasetType& getOptimizedData() const { return optimized_data_; }
DatasetType& getOptimizedData() { return optimized_data_; }
private:
std::shared_ptr<rclcpp::Node> node_; // ROS节点
DatasetType data_; // 原始数据
DatasetType optimized_data_; // 优化后的数据
HybridPipelineConfig config_; // 配置参数
// 各策略模块
std::unique_ptr<WindowingPolicyType> windowing_policy_;
std::unique_ptr<SpatialFilterPolicyType> spatial_filter_policy_;
std::unique_ptr<OutlierRejectionPolicyType> outlier_rejection_policy_;
std::unique_ptr<OptimizationPolicyType> optimization_policy_;
// 数据统计模块
std::unique_ptr<DataStats<DatasetType>> data_stats_;
};
// ===========================
// 平衡型混合管线
// ===========================
template <typename Scalar = double>
class BalancedHybridPipeline
: public HybridPipeline<
Scalar, BALData<Scalar>,
policies::implementations::NonoverlappingWindows<policies::RosLoggingPolicy>,
policies::implementations::RTreeSpatialFilter<policies::RosLoggingPolicy>,
policies::implementations::SimpleOutlierRejection<policies::RosLoggingPolicy>,
policies::implementations::CeresOptimization<policies::RosLoggingPolicy>> {
public:
explicit BalancedHybridPipeline(std::shared_ptr<rclcpp::Node> node,
BALData<Scalar> data) noexcept
: HybridPipeline<
Scalar, BALData<Scalar>,
policies::implementations::NonoverlappingWindows<policies::RosLoggingPolicy>,
policies::implementations::RTreeSpatialFilter<policies::RosLoggingPolicy>,
policies::implementations::SimpleOutlierRejection<policies::RosLoggingPolicy>,
policies::implementations::CeresOptimization<policies::RosLoggingPolicy>>(
node, std::move(data)) {}
std::string getPipelineTypeName() const override {
return "Balanced HybridPipeline (中等迭代次数)";
}
};
// ===========================
// 高性能混合管线
// ===========================
template <typename Scalar = double>
class HighPerformanceHybridPipeline
: public HybridPipeline<
Scalar, BALData<Scalar>,
policies::implementations::NonoverlappingWindows<policies::RosLoggingPolicy>,
policies::implementations::RTreeSpatialFilter<policies::RosLoggingPolicy>,
policies::implementations::SimpleOutlierRejection<policies::RosLoggingPolicy>,
policies::implementations::AdvancedOptimization<policies::RosLoggingPolicy>> {
public:
explicit HighPerformanceHybridPipeline(std::shared_ptr<rclcpp::Node> node,
BALData<Scalar> data) noexcept
: HybridPipeline<
Scalar, BALData<Scalar>,
policies::implementations::NonoverlappingWindows<policies::RosLoggingPolicy>,
policies::implementations::RTreeSpatialFilter<policies::RosLoggingPolicy>,
policies::implementations::SimpleOutlierRejection<policies::RosLoggingPolicy>,
policies::implementations::AdvancedOptimization<policies::RosLoggingPolicy>>(
node, std::move(data)) {}
std::string getPipelineTypeName() const override {
return "High-Performance HybridPipeline (高迭代次数)";
}
};
// ===========================
// 辅助函数：创建测试数据集
// ===========================
BALData<double> createTestDataset(int num_cameras, int num_points) {
std::cout << " 创建测试数据集" << std::endl;
std::cout << " 相机数: " << num_cameras << std::endl;
std::cout << " 3D点数: " << num_points << std::endl;
BALData<double> test_data;
// ============================================
// 创建相机（在圆形轨迹上）
// ============================================
std::cout << "\n[1/3] 创建相机..." << std::endl;
for (int i = 0; i < num_cameras; ++i) {
Camera cam;
cam.id = i;
// 设置相机内参
cam.model.focal = Eigen::Vector2d(800.0, 800.0);
cam.model.principal = Eigen::Vector2d(320.0, 240.0);
cam.model.distortion = Eigen::Vector2d(0.0, 0.0);
// 设置相机外参（圆形轨迹）
double angle = 2.0 * M_PI * i / num_cameras;
double radius = 5.0;
cam.t = Eigen::Vector3d(radius * std::cos(angle), radius * std::sin(angle), 1.0);
// 相机朝向中心的旋转矩阵
Eigen::AngleAxisd rotation(angle + M_PI, Eigen::Vector3d::UnitZ());
cam.R = rotation.toRotationMatrix();
test_data.cams.push_back(cam);
}
std::cout << " ✓ 创建了 " << num_cameras << " 个相机" << std::endl;
// ============================================
// 创建3D点（在中心区域随机分布）
// ============================================
std::cout << "\n[2/3] 创建3D点..." << std::endl;
for (int i = 0; i < num_points; ++i) {
Eigen::Vector3d point;
point = Eigen::Vector3d::Random() * 2.0; // 随机点在[-2,2]范围内
point(2) = std::abs(point(2)); // 确保z坐标为正
test_data.pts.push_back(point);
}
std::cout << " ✓ 创建了 " << num_points << " 个3D点" << std::endl;
// ============================================
// 生成观测（每个相机观测部分3D点）
// ============================================
std::cout << "\n[3/3] 生成观测..." << std::endl;
int obs_index = 0;
int observations_per_camera = num_points / 2; // 每个相机观测一半的点
for (int cam_idx = 0; cam_idx < num_cameras; ++cam_idx) {
const auto& camera = test_data.cams[cam_idx];
for (int pt_idx = cam_idx % 2; pt_idx < num_points; pt_idx += 2) {
Observation obs;
obs.camera_index = cam_idx;
obs.point_index = pt_idx;
// 计算投影像素
const auto& point_3d = test_data.pts[pt_idx];
// 将世界坐标转换到相机坐标
Eigen::Vector3d Pc = camera.R * point_3d + camera.t;
// 检查点是否在相机前方
if (Pc(2) > 0.1) {
// 投影到像素平面
Eigen::Vector2d projected = camera.model.project(Pc);
// 添加高斯噪声
projected = projected + Eigen::Vector2d::Random() * 1.5;
obs.pixel = projected;
test_data.obs_vec.push_back(obs);
// 更新映射关系
test_data.cam2obs[cam_idx].push_back(obs_index);
test_data.pt2obs[pt_idx].push_back(obs_index);
obs_index++;
}
}
}
std::cout << " ✓ 生成了 " << test_data.num_observations() << " 个观测" << std::endl;
std::cout << " \n" << std::endl;
return test_data;
}
// ===========================
// 主函数 - 演示完整流程
// ===========================
int main() {
std::cout << " Bundle Adjustment 混合管线演示程序 " << std::endl;
std::cout << " Hybrid Pipeline for Bundle Adjustment " << std::endl;
// 创建ROS节点
auto node = std::make_shared<rclcpp::Node>("ba_pipeline_demo");
std::cout << "✓ ROS节点已创建: " << node->get_name() << std::endl;
// 创建测试数据集
const int NUM_CAMERAS = 20; // 20个相机
const int NUM_POINTS = 50; // 50个3D点
BALData<double> test_data = createTestDataset(NUM_CAMERAS, NUM_POINTS);
// ============================================
// 测试1: 运行平衡型管线
// ============================================
std::cout << " 测试 1: 平衡型混合管线 " << std::endl;
BalancedHybridPipeline<double> balanced_pipeline(node, test_data);
balanced_pipeline.run();
// ============================================
// 测试2: 运行高性能管线
// ============================================
std::cout << " 测试 2: 高性能混合管线 " << std::endl;
// 重新创建数据（因为之前的数据已被修改）
BALData<double> test_data2 = createTestDataset(NUM_CAMERAS, NUM_POINTS);
HighPerformanceHybridPipeline<double> hp_pipeline(node, test_data2);
hp_pipeline.run();
// ============================================
// 总结
// ============================================
std::cout << " 所有测试已完成！ " << std::endl;
std::cout << " 本示例演示了: " << std::endl;
std::cout << " ✓ C++20 概念 (Concepts) 的使用 " << std::endl;
std::cout << " ✓ 策略模式 (Policy-Based Design) " << std::endl;
std::cout << " ✓ Bundle Adjustment 优化流程 " << std::endl;
std::cout << " ✓ 窗口化处理大规模数据 " << std::endl;
std::cout << " ✓ 异常值检测与剔除 " << std::endl;
std::cout << " ✓ 空间滤波技术 " << std::endl;
return 0;
}
Bundle Adjustment 混合管线演示程序
Hybrid Pipeline for Bundle Adjustment
✓ ROS节点已创建: ba_pipeline_demo
创建测试数据集
相机数: 20
3D点数: 50
[1/3] 创建相机...
✓ 创建了 20 个相机
[2/3] 创建3D点...
✓ 创建了 50 个3D点
[3/3] 生成观测...
✓ 生成了 500 个观测
测试 1: 平衡型混合管线
[参数] outlier_threshold = 3
[参数] verbose_logging = 1
[初始化] 创建 HybridPipeline
[初始化] 创建窗口策略...
[参数] window_size = 5
[初始化] 创建空间滤波策略...
[参数] spatial_distance_threshold = 5000
[初始化] 创建异常值剔除策略...
[参数] reprojection_threshold = 5
[初始化] 创建优化策略...
[参数] max_iterations = 50
[初始化] HybridPipeline 初始化完成
开始运行混合Bundle Adjustment优化管线
Pipeline Type: Balanced HybridPipeline (中等迭代次数)
【初始数据集状态】
数据集统计信息
相机数量: 20
3D点数量: 50
观测数量: 500
异常值数量: 0
异常值比例: 0.00 %
阶段 1/4: 生成优化窗口
[INFO] [ba_pipeline_demo] ====== 步骤1: 生成无重叠窗口 ======
[INFO] [ba_pipeline_demo] 创建窗口 #1: 相机范围 [0 - 4] (共 5 个相机)
[INFO] [ba_pipeline_demo] 创建窗口 #2: 相机范围 [5 - 9] (共 5 个相机)
[INFO] [ba_pipeline_demo] 创建窗口 #3: 相机范围 [10 - 14] (共 5 个相机)
[INFO] [ba_pipeline_demo] 创建窗口 #4: 相机范围 [15 - 19] (共 5 个相机)
[INFO] [ba_pipeline_demo] 窗口生成完成: 总共 4 个窗口
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口生成完成: 总共 4 个窗口
处理窗口 1/4
包含相机: 5 个
相机范围: [0 - 4]
[INFO] [ba_pipeline_demo] ====== 步骤2: 异常值剔除 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 异常值剔除完成: 0/125 观测被剔除
[INFO] [ba_pipeline_demo] 平均重投影误差: 1.097683
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤3: 空间滤波 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 空间滤波完成: 4/125 观测被标记为空间异常
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤4: Ceres优化 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机
[INFO] [ba_pipeline_demo] 初始代价: 1.397959
[INFO] [ba_pipeline_demo] 优化观测数: 121
[INFO] [ba_pipeline_demo] 优化完成:
[INFO] [ba_pipeline_demo] 初始代价: 1.397959
[INFO] [ba_pipeline_demo] 最终代价: 84.440209
[INFO] [ba_pipeline_demo] 改进: -5940.247701%
[INFO] [ba_pipeline_demo] 迭代次数: 50
[INFO] [ba_pipeline_demo] 是否收敛: 否
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口 1 处理完成
处理窗口 2/4
包含相机: 5 个
相机范围: [5 - 9]
[INFO] [ba_pipeline_demo] ====== 步骤2: 异常值剔除 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 异常值剔除完成: 50/125 观测被剔除
[INFO] [ba_pipeline_demo] 平均重投影误差: 6.800605
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤3: 空间滤波 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 空间滤波完成: 2/125 观测被标记为空间异常
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤4: Ceres优化 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机
[INFO] [ba_pipeline_demo] 初始代价: 9.162063
[INFO] [ba_pipeline_demo] 优化观测数: 75
[INFO] [ba_pipeline_demo] 优化完成:
[INFO] [ba_pipeline_demo] 初始代价: 9.162063
[INFO] [ba_pipeline_demo] 最终代价: 26.131566
[INFO] [ba_pipeline_demo] 改进: -185.214872%
[INFO] [ba_pipeline_demo] 迭代次数: 50
[INFO] [ba_pipeline_demo] 是否收敛: 否
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口 2 处理完成
处理窗口 3/4
包含相机: 5 个
相机范围: [10 - 14]
[INFO] [ba_pipeline_demo] ====== 步骤2: 异常值剔除 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 异常值剔除完成: 70/125 观测被剔除
[INFO] [ba_pipeline_demo] 平均重投影误差: 7.798222
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤3: 空间滤波 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 空间滤波完成: 0/125 观测被标记为空间异常
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤4: Ceres优化 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机
[INFO] [ba_pipeline_demo] 初始代价: 8.846148
[INFO] [ba_pipeline_demo] 优化观测数: 55
[INFO] [ba_pipeline_demo] 优化完成:
[INFO] [ba_pipeline_demo] 初始代价: 8.846148
[INFO] [ba_pipeline_demo] 最终代价: 45.830527
[INFO] [ba_pipeline_demo] 改进: -418.084561%
[INFO] [ba_pipeline_demo] 迭代次数: 50
[INFO] [ba_pipeline_demo] 是否收敛: 否
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口 3 处理完成
处理窗口 4/4
包含相机: 5 个
相机范围: [15 - 19]
[INFO] [ba_pipeline_demo] ====== 步骤2: 异常值剔除 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 异常值剔除完成: 80/125 观测被剔除
[INFO] [ba_pipeline_demo] 平均重投影误差: 9.380264
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤3: 空间滤波 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 空间滤波完成: 2/125 观测被标记为空间异常
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤4: Ceres优化 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机
[INFO] [ba_pipeline_demo] 初始代价: 10.212865
[INFO] [ba_pipeline_demo] 优化观测数: 45
[INFO] [ba_pipeline_demo] 优化完成:
[INFO] [ba_pipeline_demo] 初始代价: 10.212865
[INFO] [ba_pipeline_demo] 最终代价: 20.190565
[INFO] [ba_pipeline_demo] 改进: -97.697355%
[INFO] [ba_pipeline_demo] 迭代次数: 50
[INFO] [ba_pipeline_demo] 是否收敛: 否
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口 4 处理完成
优化流程完成
【最终数据集状态】
数据集统计信息
相机数量: 20
3D点数量: 50
观测数量: 500
异常值数量: 204
异常值比例: 40.80 %
✓✓✓ 所有优化步骤已完成 ✓✓✓
测试 2: 高性能混合管线
创建测试数据集
相机数: 20
3D点数: 50
[1/3] 创建相机...
✓ 创建了 20 个相机
[2/3] 创建3D点...
✓ 创建了 50 个3D点
[3/3] 生成观测...
✓ 生成了 500 个观测
[参数] outlier_threshold = 3.00
[参数] verbose_logging = 1
[初始化] 创建 HybridPipeline
[初始化] 创建窗口策略...
[参数] window_size = 5
[初始化] 创建空间滤波策略...
[参数] spatial_distance_threshold = 5000.00
[初始化] 创建异常值剔除策略...
[参数] reprojection_threshold = 5.00
[初始化] 创建优化策略...
[参数] advanced_max_iterations = 100
[初始化] HybridPipeline 初始化完成
开始运行混合Bundle Adjustment优化管线
Pipeline Type: High-Performance HybridPipeline (高迭代次数)
【初始数据集状态】
数据集统计信息
相机数量: 20
3D点数量: 50
观测数量: 500
异常值数量: 0
异常值比例: 0.00 %
阶段 1/4: 生成优化窗口
[INFO] [ba_pipeline_demo] ====== 步骤1: 生成无重叠窗口 ======
[INFO] [ba_pipeline_demo] 创建窗口 #1: 相机范围 [0 - 4] (共 5 个相机)
[INFO] [ba_pipeline_demo] 创建窗口 #2: 相机范围 [5 - 9] (共 5 个相机)
[INFO] [ba_pipeline_demo] 创建窗口 #3: 相机范围 [10 - 14] (共 5 个相机)
[INFO] [ba_pipeline_demo] 创建窗口 #4: 相机范围 [15 - 19] (共 5 个相机)
[INFO] [ba_pipeline_demo] 窗口生成完成: 总共 4 个窗口
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口生成完成: 总共 4 个窗口
处理窗口 1/4
包含相机: 5 个
相机范围: [0 - 4]
[INFO] [ba_pipeline_demo] ====== 步骤2: 异常值剔除 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 异常值剔除完成: 0/125 观测被剔除
[INFO] [ba_pipeline_demo] 平均重投影误差: 1.144046
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤3: 空间滤波 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 空间滤波完成: 0/125 观测被标记为空间异常
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤4: 高级优化 ======
[INFO] [ba_pipeline_demo] 使用更多迭代次数: 100
[INFO] [ba_pipeline_demo] 高级优化完成 (使用更复杂的优化策略)
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口 1 处理完成
处理窗口 2/4
包含相机: 5 个
相机范围: [5 - 9]
[INFO] [ba_pipeline_demo] ====== 步骤2: 异常值剔除 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 异常值剔除完成: 0/125 观测被剔除
[INFO] [ba_pipeline_demo] 平均重投影误差: 1.113540
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤3: 空间滤波 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 空间滤波完成: 0/125 观测被标记为空间异常
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤4: 高级优化 ======
[INFO] [ba_pipeline_demo] 使用更多迭代次数: 100
[INFO] [ba_pipeline_demo] 高级优化完成 (使用更复杂的优化策略)
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口 2 处理完成
处理窗口 3/4
包含相机: 5 个
相机范围: [10 - 14]
[INFO] [ba_pipeline_demo] ====== 步骤2: 异常值剔除 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 异常值剔除完成: 0/125 观测被剔除
[INFO] [ba_pipeline_demo] 平均重投影误差: 1.162217
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤3: 空间滤波 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 空间滤波完成: 0/125 观测被标记为空间异常
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤4: 高级优化 ======
[INFO] [ba_pipeline_demo] 使用更多迭代次数: 100
[INFO] [ba_pipeline_demo] 高级优化完成 (使用更复杂的优化策略)
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口 3 处理完成
处理窗口 4/4
包含相机: 5 个
相机范围: [15 - 19]
[INFO] [ba_pipeline_demo] ====== 步骤2: 异常值剔除 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 异常值剔除完成: 0/125 观测被剔除
[INFO] [ba_pipeline_demo] 平均重投影误差: 1.124473
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤3: 空间滤波 ======
[INFO] [ba_pipeline_demo] 处理 5 个相机的观测
[INFO] [ba_pipeline_demo] 空间滤波完成: 0/125 观测被标记为空间异常
[INFO] [ba_pipeline_demo] ====================================
[INFO] [ba_pipeline_demo] ====== 步骤4: 高级优化 ======
[INFO] [ba_pipeline_demo] 使用更多迭代次数: 100
[INFO] [ba_pipeline_demo] 高级优化完成 (使用更复杂的优化策略)
[INFO] [ba_pipeline_demo] ====================================
✓ 窗口 4 处理完成
优化流程完成
【最终数据集状态】
数据集统计信息
相机数量: 20
3D点数量: 50
观测数量: 500
异常值数量: 0
异常值比例: 0.00 %
✓✓✓ 所有优化步骤已完成 ✓✓✓
所有测试已完成！
本示例演示了:
✓ C++20 概念 (Concepts) 的使用
✓ 策略模式 (Policy-Based Design)
✓ Bundle Adjustment 优化流程
✓ 窗口化处理大规模数据
✓ 异常值检测与剔除
✓ 空间滤波技术
https://godbolt.org/z/KK77hnbdd
#include <iostream>
#include <memory>
#include <string_view>
#include <concepts>
#include <cstdint>
// ------------------------
// 日志级别枚举
// ------------------------
enum class LogLevel : std::uint8_t {
DEBUG = 0,
INFO = 1,
WARN = 2,
ERROR = 3
};
// ------------------------
// LoggingPolicy 概念
// ------------------------
template<typename Policy>
concept LoggingPolicy = requires(Policy policy, LogLevel level, std::string_view message) {
// 要求策略必须提供 log 函数，并返回 void
{ policy.log(level, message) } -> std::same_as<void>;
};
// ------------------------
// ROS 日志策略示例（这里用控制台代替 ROS 输出）
// ------------------------
class RosLoggingPolicy {
public:
explicit RosLoggingPolicy(std::shared_ptr<int> node) noexcept
: node_(std::move(node)) {} // 这里用 int 模拟 ROS 节点
// 满足 LoggingPolicy 概念
void log(LogLevel level, std::string_view message) noexcept {
if(!node_) return;
switch(level) {
case LogLevel::DEBUG:
std::cout << "[DEBUG] " << message << "\n";
break;
case LogLevel::INFO:
std::cout << "[INFO] " << message << "\n";
break;
case LogLevel::WARN:
std::cout << "[WARN] " << message << "\n";
break;
case LogLevel::ERROR:
std::cout << "[ERROR] " << message << "\n";
break;
}
}
private:
std::shared_ptr<int> node_; // 模拟 ROS 节点指针
};
// 静态断言：检查 RosLoggingPolicy 是否满足 LoggingPolicy 概念
static_assert(LoggingPolicy<RosLoggingPolicy>);
// ------------------------
// 使用示例：Pipeline 模板类
// ------------------------
template<LoggingPolicy Logger>
class ExamplePipeline {
public:
explicit ExamplePipeline(Logger logger) : logger_(logger) {}
void run() {
logger_.log(LogLevel::INFO, "Pipeline started");
logger_.log(LogLevel::DEBUG, "Performing step 1...");
logger_.log(LogLevel::WARN, "Step 2 might be slow");
logger_.log(LogLevel::ERROR, "Step 3 failed (simulated)");
logger_.log(LogLevel::INFO, "Pipeline finished");
}
private:
Logger logger_;
};
// ------------------------
// 主函数
// ------------------------
int main() {
// 模拟 ROS 节点
auto node = std::make_shared<int>(42);
// 创建日志策略
RosLoggingPolicy logger(node);
// 创建 Pipeline，并注入日志策略
ExamplePipeline<RosLoggingPolicy> pipeline(logger);
// 运行 Pipeline，会打印日志
pipeline.run();
return 0;
}
✓ 说明
- LogLevel 枚举
- 定义日志等级：DEBUG, INFO, WARN, ERROR
- 使用
uint8_t存储节省内存
- LoggingPolicy 概念
- 要求类型提供
log(LogLevel, std::string_view)函数，并返回void - 可以保证模板中使用策略类型时类型安全
- 要求类型提供
- RosLoggingPolicy
- 实现了
LoggingPolicy概念 - 这里用控制台打印代替 ROS 日志
- 可以替换成真实
rclcpp::Node节点输出
- 实现了
- Pipeline 使用
- Pipeline 类模板接收任意满足
LoggingPolicy的策略类型 - 内部调用
logger_.log(...)输出不同级别日志 - 体现了策略模式（Strategy Pattern）
- Pipeline 类模板接收任意满足
- 运行效果
[INFO] Pipeline started [DEBUG] Performing step 1... [WARN] Step 2 might be slow [ERROR] Step 3 failed (simulated) [INFO] Pipeline finished
https://wandbox.org/permlink/dZojlzstbXzgJ3H0
1⃣ CameraWindow 和 WindowingResult
// ------------------------
// 摄像机窗口结构体
// ------------------------
struct CameraWindow {
std::vector<int> camera_indices; // 窗口中包含的相机索引
int start_camera_id = -1; // 窗口起始相机 ID
size_t window_size = 0; // 窗口大小（相机数量）
// ------------------------
// 判断窗口是否为空
// ------------------------
[[nodiscard]] bool empty() const noexcept { return camera_indices.empty(); }
// 窗口内相机数量
[[nodiscard]] size_t size() const noexcept { return camera_indices.size(); }
// 迭代器支持，方便 range-based for
[[nodiscard]] auto begin() const noexcept { return camera_indices.begin(); }
[[nodiscard]] auto end() const noexcept { return camera_indices.end(); }
};
// ------------------------
// 窗口策略返回结果
// ------------------------
struct WindowingResult {
std::vector<CameraWindow> windows; // 所有生成的窗口
size_t total_cameras_covered = 0; // 覆盖的相机总数
size_t overlap_count = 0; // 窗口重叠数量
[[nodiscard]] bool empty() const noexcept { return windows.empty(); }
[[nodiscard]] size_t num_windows() const noexcept { return windows.size(); }
};
理解：
- 在 Bundle Adjustment（BA）中，为了提高效率，通常不对所有相机和点同时优化，而是将相机分成小窗口逐步优化。
- 每个
CameraWindow保存窗口中的相机索引集合。 WindowingResult保存整个数据集切分后的所有窗口信息。
2⃣ WindowingPolicy 概念
template<typename Policy, typename Dataset>
concept WindowingPolicy = requires(Policy policy, const Dataset& dataset) {
requires DatasetLike<Dataset>; // 数据集必须满足 DatasetLike 概念
{ policy.generate_windows(dataset) } -> std::same_as<WindowingResult>;
requires std::destructible<Policy>;
};
理解：
- 一个 窗口策略 必须提供
generate_windows(dataset)函数 - 返回类型为
WindowingResult - 用于将相机划分为不同优化窗口
数学上可以理解为：
Windows = generate_windows ( dataset ) \text{Windows} = \text{generate\_windows}(\text{dataset}) Windows=generate_windows(dataset)
3⃣ OutlierRejectionResult 和策略概念
// ------------------------
// 异常值剔除结果
// ------------------------
struct OutlierRejectionResult {
size_t total_observations_processed = 0; // 总处理观测数
size_t total_inliers_found = 0; // 内点数量
// 默认构造函数
OutlierRejectionResult() = default;
// 内点比率
[[nodiscard]] double inlier_ratio() const noexcept {
return total_observations_processed > 0 ?
static_cast<double>(total_inliers_found) / total_observations_processed
: 0.0;
}
};
// ------------------------
// 异常值剔除策略概念
// ------------------------
template<typename Policy, typename Dataset>
concept OutlierRejectionPolicy = requires(Policy policy, Dataset& dataset, std::span<const int> camera_window) {
requires DatasetLike<Dataset>;
{ policy.rejectOutliers(dataset, camera_window) } -> std::same_as<OutlierRejectionResult>;
{ policy.get_algorithm_name() } -> std::convertible_to<std::string_view>;
{ policy.get_statistics() } -> std::convertible_to<std::string>;
requires std::destructible<Policy>;
};
理解：
- 异常值剔除的目的是去掉观测中投影误差过大的点
- 内点比率：
inlier_ratio = total_inliers_found total_observations_processed \text{inlier\_ratio} = \frac{\text{total\_inliers\_found}}{\text{total\_observations\_processed}} inlier_ratio=total_observations_processedtotal_inliers_found - 策略要求提供：
rejectOutliers(dataset, camera_window)：剔除窗口内异常观测get_algorithm_name()：算法名称get_statistics()：统计信息
4⃣ 异常值剔除流程（并行处理示意）
// 对每个窗口中的相机进行处理
void processCamerasParallel(DatasetLike auto& dataset, std::span<const int> camera_window,
auto& result_internal)
{
// 将相机索引转成相机对象
auto window_cameras = camera_window
| std::views::transform([&dataset](int idx){ return dataset.cameras()[idx]; });
// 并行处理每个相机
std::for_each(std::execution::par, window_cameras.begin(), window_cameras.end(),
[&dataset, &result_internal](const auto& camera) {
processSingleCamera(dataset, camera, result_internal);
});
}
理解：
- 利用 并行算法（
std::execution::par）加速异常值剔除 - 对每个相机计算重投影误差，并更新内点/外点标记
- 数学公式：对于观测
o
o
o，相机
c
c
c 和三维点
P
P
P：
error ∗ o = ∣ u ∗ obs − π c ( P ) ∣ \text{error}*o = | u*{\text{obs}} - \pi_c(P) | error∗o=∣u∗obs−πc(P)∣
如果 error o > 阈值 \text{error}_o > \text{阈值} erroro>阈值，标记为外点。
5⃣ SpatialFilterResult 和策略概念
// ------------------------
// 空间滤波结果
// ------------------------
struct SpatialFilterResult {
size_t total_observations_processed = 0;
size_t total_observations_kept = 0;
SpatialFilterResult() = default;
// 保留率
[[nodiscard]] double retention_ratio() const noexcept {
return total_observations_processed > 0 ?
static_cast<double>(total_observations_kept) / total_observations_processed
: 0.0;
}
};
// ------------------------
// 空间滤波策略概念
// ------------------------
template<typename Policy, typename Dataset>
concept SpatialFilterPolicy = requires(Policy policy, Dataset& dataset, std::span<const int> camera_window) {
requires DatasetLike<Dataset>;
{ policy.filterSpatially(dataset, camera_window) } -> std::same_as<SpatialFilterResult>;
{ policy.getLastStatistics() } -> std::convertible_to<std::string>;
requires std::destructible<Policy>;
};
理解：
- 空间滤波主要用于去除不合理观测（例如太靠近边缘或者空间分布异常）
- 保留率：
retention_ratio = total_observations_kept total_observations_processed \text{retention\_ratio} = \frac{\text{total\_observations\_kept}}{\text{total\_observations\_processed}} retention_ratio=total_observations_processedtotal_observations_kept - 策略要求提供：
filterSpatially(dataset, camera_window)：对窗口内观测进行空间滤波getLastStatistics()：输出统计信息
总结
- 窗口策略（WindowingPolicy）
- 将相机划分窗口，加速优化
- 输出
WindowingResult
- 异常值剔除策略（OutlierRejectionPolicy）
- 对窗口内观测计算重投影误差
- 剔除误差过大的外点
- 输出
OutlierRejectionResult
- 空间滤波策略（SpatialFilterPolicy）
- 对窗口内观测根据空间分布筛选
- 输出
SpatialFilterResult
- 数学公式核心
- 重投影误差：
error ∗ o = ∣ u ∗ obs − π c ( P ) ∣ \text{error}*o = | u*{\text{obs}} - \pi_c(P) | error∗o=∣u∗obs−πc(P)∣ - 内点比率：
inlier_ratio = 内点数 总观测数 \text{inlier\_ratio} = \frac{\text{内点数}}{\text{总观测数}} inlier_ratio=总观测数内点数 - 空间保留率：
retention_ratio = 保留观测数 总观测数 \text{retention\_ratio} = \frac{\text{保留观测数}}{\text{总观测数}} retention_ratio=总观测数保留观测数
- 重投影误差：
- 并行计算
- 对每个窗口中的相机同时处理，提高效率
// ------------------------
// 空间滤波结果
// ------------------------
struct SpatialFilterResult {
size_t total_observations_processed = 0; // 总处理的观测数
size_t total_observations_kept = 0; // 保留的观测数
SpatialFilterResult() = default;
// 计算保留率 (Retention Ratio)
[[nodiscard]] double retention_ratio() const noexcept {
return total_observations_processed > 0 ?
static_cast<double>(total_observations_kept) / total_observations_processed
: 0.0;
}
};
// ------------------------
// 空间滤波策略概念
// ------------------------
template<typename Policy, typename Dataset>
concept SpatialFilterPolicy = requires(Policy policy, Dataset& dataset, std::span<const int> camera_window) {
requires DatasetLike<Dataset>; // 数据集必须满足 DatasetLike 概念
// 主空间滤波方法：返回 SpatialFilterResult
{ policy.filterSpatially(dataset, camera_window) } -> std::same_as<SpatialFilterResult>;
// 获取上一次统计信息
{ policy.getLastStatistics() } -> std::convertible_to<std::string>;
requires std::destructible<Policy>; // 策略必须可析构
};
// ------------------------
// 空间滤波策略实现：R-tree + k近邻 + Z-score
// ------------------------
template <typename Dataset>
requires DatasetLike<Dataset>
void buildSpatialIndex(Dataset& dataset) {
std::lock_guard<std::mutex> lock(mutex_);
if (config_.verbose_logging) {
logging_policy_.log(LogLevel::INFO, "Building R-tree spatial index");
}
rtree_.clear();
// 构建 R-tree 空间索引，存储所有 3D 点
size_t count = 0;
const auto& points = dataset.points();
for (const auto& point : points) {
if (point.size() >= 3) {
rtree_.insert(std::make_pair(Point3D(point[0], point[1], point[2]), count++));
}
}
rtree_built_ = true;
if (config_.verbose_logging) {
logging_policy_.log(LogLevel::INFO,
"R-tree built with " + std::to_string(count) + " points");
}
}
// ------------------------
// 空间滤波主逻辑：只处理内点 (inliers)
// ------------------------
for (int obs_idx : obs_indices_set) {
if (obs_idx >= 0 && obs_idx < static_cast<int>(observations.size())) {
const auto& obs = observations[obs_idx];
if (!obs.outlier) { // 只处理内点
inlier_obs_indices.push_back(obs_idx);
// 并行处理每个观测，更新保留计数
std::atomic<size_t> kept_count{0};
std::for_each(std::execution::par,
inlier_obs_indices.begin(),
inlier_obs_indices.end(),
[&dataset, &kept_count](int obs_idx) {
bool is_valid = processSingleObservation(dataset, obs_idx);
if (is_valid) kept_count.fetch_add(1);
});
}
}
}
// ------------------------
// k近邻统计与 z-score 异常值检测
// ------------------------
std::vector<Value> neighbors;
rtree_.query(boost::geometry::index::nearest(query_pt, config_.k_neighbors + 1),
std::back_inserter(neighbors));
// 过滤掉查询点本身
auto distances = neighbors
| std::views::filter([point_idx = obs.point_index](const auto& neighbor) {
return static_cast<int>(neighbor.second) != point_idx;
})
| std::views::transform([&query_pt](const auto& neighbor){
return boost::geometry::distance(query_pt, neighbor.first);
})
| std::ranges::to<std::vector>();
// 计算平均值和标准差
auto [distance_sum, squared_distance_sum, num_distances] = std::ranges::fold_left(
distances,
std::tuple{0.0, 0.0, 0u},
[](auto previous_result, double distance_value) {
auto& [total_distance_sum, total_squared_distance_sum, distance_count] = previous_result;
total_distance_sum += distance_value;
total_squared_distance_sum += distance_value * distance_value;
++distance_count;
return previous_result;
}
);
double mean = distance_sum / num_distances; // 平均距离
double variance = (squared_distance_sum - num_distances * mean * mean) / num_distances; // 方差
double stddev = std::sqrt(variance); // 标准差
// 最近邻距离与 z-score 检查
double nearest_distance = std::ranges::min(distances);
bool is_valid = stddev > 0.0 &&
std::abs(nearest_distance - mean) <= config_.std_dev_multiplier * stddev;
理解总结
- R-tree 空间索引
- 将三维点构建成树状结构以加速 k最近邻查询
- 查询复杂度为 O ( log n ) O(\log n) O(logn)
- 处理观测
- 只对 内点（inliers）进行空间滤波
- 并行处理每个观测，提高性能
- 统计距离与 z-score 异常值检测
- 对每个点查询 k k k 个最近邻
- 计算均值
m
e
a
n
mean
mean 和标准差
s
t
d
d
e
v
stddev
stddev：
m e a n = ∑ i = 1 k d i k , v a r i a n c e = ∑ i = 1 k d i 2 − k ⋅ m e a n 2 k , s t d d e v = v a r i a n c e mean = \frac{\sum_{i=1}^{k} d_i}{k},\quad variance = \frac{\sum_{i=1}^{k} d_i^2 - k \cdot mean^2}{k},\quad stddev = \sqrt{variance} mean=k∑i=1kdi,variance=k∑i=1kdi2−k⋅mean2,stddev=variance - 判断最近邻距离是否合理：
∣ d n e a r e s t − m e a n ∣ ≤ std_dev_multiplier ⋅ s t d d e v |d_{nearest} - mean| \leq \text{std\_dev\_multiplier} \cdot stddev ∣dnearest−mean∣≤std_dev_multiplier⋅stddev
- 保留观测统计
total_observations_processed：处理的总观测数total_observations_kept：通过空间滤波保留下来的观测数retention_ratio= 保留率
- 并行处理
- 使用
std::for_each+std::execution::par加速计算 - 使用
std::atomic确保计数线程安全
- 使用
1. 背景理解
这里展示的是两种数据集在 空间滤波 + 异常值剔除 (Outlier Rejection) 后的统计信息。核心目标是剔除重投影误差过大或空间异常的观测点，保证 Bundle Adjustment (BA) 优化的稳定性。
- Ladybug dataset 和 Trafalgar Square dataset 是两种不同的多视图数据集。
- 对每个数据集，策略使用了：
- Reprojection Error Threshold: 观测点投影到相机像素平面后，允许的最大误差，例如 25.0 pix 25.0 \text{pix} 25.0pix
- Neighbours for spatial filter: 空间滤波时参考的近邻点数，例如 100 100 100
- std_dev multiplier: z-score 异常值检测中，标准差倍数，例如 4.5 4.5 4.5
2. Ladybug dataset 过滤统计
Thresholds:
- Reprojection error threshold : 25.0 pix
- Neighbours for spatial filter : 100
- std_dev multiplier : 4.5
Outliers:
- Count = 22866 / 678155 (3.371796%)
- ReprojError:
Mean = 21.980313 px
StdDev = 36.999753 px
Median = 19.146612 px
解释
- 阈值设置
- 投影误差阈值 T reproj = 25.0 T_\text{reproj} = 25.0 Treproj=25.0 像素
- 空间滤波使用 k = 100 k = 100 k=100 个近邻
- z-score 异常值检测倍数 m = 4.5 m = 4.5 m=4.5
- 异常值统计
- 总观测点： N total = 678155 N_\text{total} = 678155 Ntotal=678155
- 剔除异常值： N outlier = 22866 N_\text{outlier} = 22866 Noutlier=22866
- 剔除比例：
Outlier% = 22866 678155 ⋅ 100 % ≈ 3.37 % \text{Outlier\%} = \frac{22866}{678155} \cdot 100\% \approx 3.37\% Outlier%=67815522866⋅100%≈3.37%
- 重投影误差统计
- 平均误差：
e ˉ = 21.98 px \bar{e} = 21.98 \text{px} eˉ=21.98px - 标准差：
σ e = 36.99 px \sigma_e = 36.99 \text{px} σe=36.99px - 中位数：
e ~ = 19.15 px \tilde{e} = 19.15 \text{px} e~=19.15px
- 平均误差：
- 理解
- 平均误差略低于阈值，说明大部分观测点是内点。
- 标准差较大，说明仍有少量极端观测点（outlier）被剔除。
3. Trafalgar Square dataset 过滤统计
Thresholds:
- Reprojection error threshold : 25.0 pix
- Neighbours for spatial filter : 100
- std_dev multiplier : 4.5
Outliers:
- Count = 20114 / 225267 (8.928960%)
- ReprojError:
Mean = 37.676941 px
StdDev = 19.108898 px
Median = 34.209722 px
解释
- 阈值设置
- 同样使用 T reproj = 25.0 T_\text{reproj} = 25.0 Treproj=25.0 pix， k = 100 k=100 k=100， m = 4.5 m=4.5 m=4.5
- 异常值统计
- 总观测点： N total = 225267 N_\text{total} = 225267 Ntotal=225267
- 剔除异常值： N outlier = 20114 N_\text{outlier} = 20114 Noutlier=20114
- 剔除比例：
Outlier% = 20114 225267 ⋅ 100 % ≈ 8.93 % \text{Outlier\%} = \frac{20114}{225267} \cdot 100\% \approx 8.93\% Outlier%=22526720114⋅100%≈8.93%
- 重投影误差统计
- 平均误差：
e ˉ = 37.68 px \bar{e} = 37.68 \text{px} eˉ=37.68px - 标准差：
σ e = 19.11 px \sigma_e = 19.11 \text{px} σe=19.11px - 中位数：
e ~ = 34.21 px \tilde{e} = 34.21 \text{px} e~=34.21px
- 平均误差：
- 理解
- 平均误差已经超过阈值 25 25 25 px，说明数据集本身存在较多噪声。
- 异常值比例约 9%，比 Ladybug dataset 高，空间滤波与 outlier rejection 起到关键作用。
4. 公式总结
- Outlier 比例：
Outlier% = N outlier N total ⋅ 100 % \text{Outlier\%} = \frac{N_\text{outlier}}{N_\text{total}} \cdot 100\% Outlier%=NtotalNoutlier⋅100% - 平均重投影误差：
e ˉ = 1 N inlier ∑ i = 1 N inlier e i \bar{e} = \frac{1}{N_\text{inlier}} \sum_{i=1}^{N_\text{inlier}} e_i eˉ=Ninlier1i=1∑Ninlierei - 重投影误差标准差：
σ e = 1 N inlier ∑ i = 1 N inlier ( e i − e ˉ ) 2 \sigma_e = \sqrt{\frac{1}{N_\text{inlier}} \sum_{i=1}^{N_\text{inlier}} (e_i - \bar{e})^2} σe=Ninlier1i=1∑Ninlier(ei−eˉ)2 - 中位数：
e ~ = median ( e 1 , e 2 , … , e N inlier ) \tilde{e} = \text{median}({e_1, e_2, \dots, e_{N_\text{inlier}}}) e~=median(e1,e2,…,eNinlier)
5. 关键理解
- 重投影误差衡量的是三维点投影回像素平面后的误差大小。
- 空间滤波通过 R-tree 查询 k 近邻计算距离的 z-score，剔除明显异常点。
- 异常值剔除与阈值密切相关，阈值越低，剔除比例越高。
- 不同数据集噪声水平不同，需要根据数据集调整 T reproj T_\text{reproj} Treproj 和 std_dev_multiplier。
1. 优化策略数据结构
struct IterationSummary {
double cost = 0.0; // 当前迭代的目标函数值
double cost_change = 0.0; // 本次迭代目标函数变化量
double cost_reduction_percentage = 0.0; // 本次迭代成本减少百分比
bool step_is_successful = false; // 本次迭代是否成功
bool success = false; // 整个优化是否成功
double initial_cost = 0.0; // 优化初始成本
double final_cost = 0.0; // 优化最终成本
std::vector<IterationSummary> iteration_summaries; // 每次迭代的详细信息
std::string solver_summary; // 优化器总结信息
};
理解：
cost： r ( x ) r(x) r(x) 的平方和，即重投影误差的总和cost_change：本次迭代目标函数减少量cost_reduction_percentage：
cost_reduction_percentage = initial_cost − final_cost initial_cost ⋅ 100 % \text{cost\_reduction\_percentage} = \frac{\text{initial\_cost} - \text{final\_cost}}{\text{initial\_cost}} \cdot 100\% cost_reduction_percentage=initial_costinitial_cost−final_cost⋅100%step_is_successful：迭代步是否有效iteration_summaries：每一次迭代的详细信息
2. 优化策略概念
template<typename Policy, typename Dataset>
concept OptimizationPolicy = requires(
Policy policy,
Dataset& dataset,
std::span<const int> camera_window)
{
requires DatasetLike<Dataset>;
{ policy.optimize(dataset, camera_window) } -> std::same_as<OptimizationResult>;
{ policy.getLastStatistics() } -> std::convertible_to<std::string>;
{ policy.reset() } -> std::same_as<void>;
requires std::destructible<Policy>;
};
理解：
- 该概念约束了优化策略必须实现：
optimize(dataset, camera_window)：针对选定相机窗口执行 BA 优化，返回OptimizationResultgetLastStatistics()：返回最近一次优化的统计信息字符串reset()：重置策略状态
DatasetType必须满足 DatasetLike 概念std::destructible<Policy>：确保策略可正常析构
3. 优化问题描述
输入数据
- 观测点 ( u i j , v i j ) (u_{ij}, v_{ij}) (uij,vij)：第 i i i 个相机观察到的第 j j j 个 3D 点的像素坐标
- 初始估计：
- 相机参数：
R ∗ i , t ∗ i (外参) , f i , k ∗ 1 i , k ∗ 2 i (内参) \mathbf{R}*i, \mathbf{t}*i \quad \text{(外参)}, \quad f_i, k*{1i}, k*{2i} \quad \text{(内参)} R∗i,t∗i(外参),fi,k∗1i,k∗2i(内参) - 3D 点：
X j = ( x j , y j , z j ) \mathbf{X}_j = (x_j, y_j, z_j) Xj=(xj,yj,zj)
- 相机参数：
优化目标
- 优化相机外参 R , t \mathbf{R}, \mathbf{t} R,t、内参 f , k 1 , k 2 f, k_1, k_2 f,k1,k2
- 优化三维点位置 ( x , y , z ) (x, y, z) (x,y,z)
- 最小化重投影误差：
min x ∣ r ( x ) ∣ ∗ 2 2 \min_x | r(x) |*2^2 xmin∣r(x)∣∗22
其中 r ( x ) r(x) r(x) 表示观测点的重投影残差：
r ∗ i j ( x ) = project ( R i , t ∗ i , X ∗ j ) − ( u ∗ i j , v ∗ i j ) r*{ij}(x) = \text{project}(\mathbf{R}_i, \mathbf{t}*i, \mathbf{X}*j) - (u*{ij}, v*{ij}) r∗ij(x)=project(Ri,t∗i,X∗j)−(u∗ij,v∗ij)
4. 优化方法步骤
4.1 线性化
- 对当前估计点 x x x，计算残差关于参数的雅可比矩阵 J J J
- 使用 一阶泰勒展开：
f ( x + δ ) ≈ f ( x ) + J δ f(x + \delta) \approx f(x) + J \delta f(x+δ)≈f(x)+Jδ - 重投影误差的线性化问题：
min ∣ r ( x + δ ) ∣ 2 2 → min ∣ r ( x ) + J δ ∣ 2 2 \min | r(x + \delta) |_2^2 \rightarrow \min | r(x) + J \delta |_2^2 min∣r(x+δ)∣22→min∣r(x)+Jδ∣22 - 正规方程 (Normal Equation)：
J T J , δ = − J T r J^T J , \delta = - J^T r JTJ,δ=−JTr
4.2 线性子问题求解
- Schur Complement 技术：
- 利用雅可比矩阵稀疏性
- 消去点变量，只解相机参数小系统
- 再通过回代求 3D 点位置
4.3 信赖域策略 (Trust-Region)
- 使用 Levenberg-Marquardt 或 Dogleg 方法：
- 防止在非线性或条件不良情况下，迭代步过大
- 权衡 Gauss-Newton 方法的速度和梯度下降法的稳定性
4.4 参数更新
- 计算增量
δ
\delta
δ 后，更新参数：
x ← x + δ x \leftarrow x + \delta x←x+δ - 重复迭代直到收敛：
- ∣ δ ∣ < ϵ | \delta | < \epsilon ∣δ∣<ϵ
- 或最大迭代次数达到
5. 关键公式总结
- 残差定义：
r i j = project ( R i , t ∗ i , X ∗ j ) − ( u ∗ i j , v ∗ i j ) r_{ij} = \text{project}(\mathbf{R}_i, \mathbf{t}*i, \mathbf{X}*j) - (u*{ij}, v*{ij}) rij=project(Ri,t∗i,X∗j)−(u∗ij,v∗ij) - 优化目标：
min x ∣ r ( x ) ∣ ∗ 2 2 = ∑ ∗ i , j ∣ r i j ∣ 2 2 \min_x | r(x) |*2^2 = \sum*{i,j} | r_{ij} |_2^2 xmin∣r(x)∣∗22=∑∗i,j∣rij∣22 - 一阶泰勒展开：
r ( x + δ ) ≈ r ( x ) + J δ r(x + \delta) \approx r(x) + J \delta r(x+δ)≈r(x)+Jδ - 正规方程：
J T J , δ = − J T r J^T J , \delta = - J^T r JTJ,δ=−JTr - 信赖域更新：
x ← x + δ x \leftarrow x + \delta x←x+δ
6. 理解总结
- BA 优化就是不断调整相机姿态和三维点位置，使重投影误差最小
- 线性化 + Schur Complement + Trust-Region 是现代 BA 求解器核心
- 每次迭代都会记录
IterationSummary，方便调试和统计收敛情况 - 适用于 Pipeline 的 窗口优化策略：只在窗口内相机和点进行优化，提高计算效率
1. 优化器配置
if (config_.fix_first_camera && storage_info.cam_count > 0
&& !camera_params_storage_empty())
{
// 将第一个相机参数固定（不优化），常用于 BA 参考坐标系
problem.SetParameterBlockConstant(camera_params_storage_[0].data());
}
// Ceres Solver 配置
ceres::Solver::Options options;
options.minimizer_type = ceres::TRUST_REGION; // 使用信赖域法
options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; // Levenberg-Marquardt
options.linear_solver_type = ceres::SPARSE_SCHUR; // 稀疏 Schur 求解
options.preconditioner_type = ceres::SCHUR_JACOBI; // 预处理器类型
options.num_threads = config_.num_threads; // 并行线程数
options.minimizer_progress_to_stdout = config_.minimizer_progress_to_stdout; // 输出进度
options.max_num_iterations = config_.max_num_iterations; // 最大迭代次数
options.function_tolerance = config_.function_tolerance; // 目标函数容忍度
options.gradient_tolerance = config_.gradient_tolerance; // 梯度容忍度
options.parameter_tolerance = config_.parameter_tolerance; // 参数变化容忍度
options.logging_type = ceres::PER_MINIMIZER_ITERATION; // 每次迭代输出日志
理解：
- 信赖域方法（Trust Region）：
- 避免大步长导致不收敛
- Levenberg-Marquardt：
- 综合 Gauss-Newton 的快速收敛和梯度下降的稳定性
- SPARSE_SCHUR：
- 利用雅可比矩阵稀疏性，通过 Schur Complement 只解相机参数子系统
- 固定第一个相机：
- 避免整体平移/旋转自由度的不确定性
2. 相机参数提取和验证
auto cameras_with_observations = camera_window_span
| std::views::filter([&](int camera_index) {
return camera_index >= 0 && camera_index < static_cast<int>(in_dataset.cameras().size());
})
| std::views::transform([&](int camera_index) {
return in_dataset.cameras()[camera_index].id;
})
| std::views::filter([&](int camera_id){
auto obs_it = camera_to_observations.find(camera_id);
return obs_it != camera_to_observations.end() && !obs_it->second.empty();
});
for(int camera_id : cameras_with_observations)
{
// 在数据集中查找对应相机
auto cam_it = std::ranges::find_if(in_dataset.cameras(),
[camera_id](const auto& cam){ return cam.id == camera_id; });
if (cam_it == in_dataset.cameras().end()) continue;
// 提取相机参数
const auto& cam = *cam_it;
auto q = cam.rotation().unit_quaternion(); // 使用四元数表示旋转
auto t = cam.translation(); // 平移向量
auto k = cam.intrinsics.as_vec3(); // 内参 f, k1, k2
// 创建相机参数向量: 10 个参数
std::vector<double> cam_params = {
static_cast<double>(q.x()), static_cast<double>(q.y()), static_cast<double>(q.z()), static_cast<double>(q.w()),
static_cast<double>(t.x()), static_cast<double>(t.y()), static_cast<double>(t.z()),
static_cast<double>(k[0]), static_cast<double>(k[1]), static_cast<double>(k[2])
};
// 验证参数是否有效（有限数）
bool valid = std::ranges::all_of(cam_params, [](double p){ return std::isfinite(p); });
if (!valid) continue;
// 存储参数并添加到 Ceres 问题
camera_id_to_storage_idx[camera_id] = camera_params_storage_.size();
camera_params_storage_.push_back(std::move(cam_params));
double* cam_ptr = camera_params_storage_.back().data();
problem.AddParameterBlock(cam_ptr, 10);
}
理解：
- 过滤窗口内无效相机索引（<0 或超出范围）
- 使用 四元数 参数化旋转，避免欧拉角万向节锁问题
- 相机参数总共 10 维：
cam_params = [ q x , q y , q z , q w , t x , t y , t z , f , k 1 , k 2 ] \text{cam\_params} = [q_x, q_y, q_z, q_w, t_x, t_y, t_z, f, k_1, k_2] cam_params=[qx,qy,qz,qw,tx,ty,tz,f,k1,k2] - 验证有限性，剔除 NaN/无穷值
- 添加到 Ceres 的 ParameterBlock
3. 点参数提取和验证
auto valid_observations = obs_indices
| std::views::filter([&](int idx){ return idx >=0 && idx < static_cast<int>(observations.size()); })
| std::views::filter([&](int idx){
const auto& obs = observations[idx];
if (obs.outlier) { outlier_filtered++; return false; }
return true;
})
| std::views::filter([&](int idx){ return observations[idx].camera_index == camera_id; })
| std::views::filter([&](int idx){
const auto& obs = observations[idx];
return obs.point_index >=0 && obs.point_index < static_cast<int>(points.size());
});
for(int obs_idx : valid_observations)
{
total_observations_processed++;
const auto& obs = observations[obs_idx];
size_t pt_storage_idx;
auto pit = point_index_to_storage_idx.find(obs.point_index);
if (pit == point_index_to_storage_idx.end())
{
// 创建新的点参数块
const auto& p = points[obs.point_index];
if (!std::ranges::all_of(p, [](double coord){ return std::isfinite(coord); })) continue;
pt_storage_idx = point_params_storage_.size();
point_index_to_storage_idx[obs.point_index] = pt_storage_idx;
point_params_storage_.push_back({p[0], p[1], p[2]});
double* pt_ptr = point_params_storage_.back().data();
problem.AddParameterBlock(pt_ptr, 3);
}
else
{
pt_storage_idx = pit->second;
}
}
理解：
- 过滤条件：
- 观测索引有效
- 非异常值
- 相机索引匹配
- 点索引有效
- 对每个有效点：
- 检查坐标有限性
- 如果是新点，创建 ParameterBlock（3 维： x , y , z x, y, z x,y,z）
- 已存在的点复用 ParameterBlock
数学公式：
- 相机参数块 x i ∈ R 10 x_i \in \mathbb{R}^{10} xi∈R10
- 3D 点参数块 X j ∈ R 3 X_j \in \mathbb{R}^3 Xj∈R3
- 对每个观测
(
i
,
j
)
(i,j)
(i,j) 构建残差：
r i j ( x i , X j ) = project ( x i , X j ) − ( u i j , v i j ) r_{ij}(x_i, X_j) = \text{project}(x_i, X_j) - (u_{ij}, v_{ij}) rij(xi,Xj)=project(xi,Xj)−(uij,vij) - BA 优化问题：
min x i , X j ∑ ( i , j ) ∈ observations ∣ r i j ( x i , X j ) ∣ 2 \min_{{x_i}, {X_j}} \sum_{(i,j) \in \text{observations}} | r_{ij}(x_i, X_j) |^2 xi,Xjmin(i,j)∈observations∑∣rij(xi,Xj)∣2
✓ 总结理解：
- 固定第一个相机 用作参考
- 相机参数 10 维：四元数 + 平移 + 内参
- 3D 点参数 3 维
- 有效性检查：剔除 NaN / inf
- Ceres 参数块：
problem.AddParameterBlock- 相机 10 维
- 点 3 维
- 残差函数：
r i j = project ( x i , X j ) − ( u i j , v i j ) r_{ij} = \text{project}(x_i, X_j) - (u_{ij}, v_{ij}) rij=project(xi,Xj)−(uij,vij)
1. 相机优化结果写回
for (const auto& [camera_id, storage_idx] : valid_camera_updates)
{
// 在数据集中查找对应相机
auto cam_it = std::ranges::find_if(dataset.cameras(),
[camera_id](const auto& cam) { return cam.id == camera_id; });
if (cam_it == dataset.cameras().end()) continue;
// 从优化存储中取出参数块
std::span<const double, 10> params{ camera_params_storage_[storage_idx] };
// 四元数旋转 (w,x,y,z)
Eigen::Quaterniond q(params[3], params[0], params[1], params[2]);
q.normalize(); // 保证单位四元数
// 平移向量
Eigen::Vector3d t(params[4], params[5], params[6]);
// 更新相机位姿 (T_c_w)
cam_it->T_c_w = typename Dataset::Traits::SE3(
typename Dataset::Traits::SO3(q), t
);
// 更新相机内参
cam_it->intrinsics.focal_length = params[7];
cam_it->intrinsics.k1 = params[8];
cam_it->intrinsics.k2 = params[9];
}
理解：
- 查找相机对象：
- 根据
camera_id在数据集里找到对应相机
- 根据
- 提取优化后的参数：
- 相机参数存储在
camera_params_storage_中，每个相机 10 维
[ q x , q y , q z , q w , t x , t y , t z , f , k 1 , k 2 ] [q_x, q_y, q_z, q_w, t_x, t_y, t_z, f, k_1, k_2] [qx,qy,qz,qw,tx,ty,tz,f,k1,k2]
- 相机参数存储在
- 四元数归一化：
- 确保旋转矩阵合法
- 单位四元数约束：$ |q| = 1 $
- 更新相机位姿：
T c w = [ R t 0 1 ] , R = SO3 ( q ) , t = ( t x , t y , t z ) T T_{c}^{w} = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}, \quad R = \text{SO3}(q), \\ t = (t_x, t_y, t_z)^T Tcw=[R0t1],R=SO3(q),t=(tx,ty,tz)T - 更新内参：
- 焦距
f，径向畸变系数k1、k2
- 焦距
2. 3D 点优化结果写回
auto valid_point_updates = point_index_to_storage_idx
| std::views::filter([&](const auto& pair) {
const auto& [point_index, storage_idx] = pair;
return storage_idx < point_params_storage_.size() &&
point_index >= 0 &&
point_index < static_cast<int>(dataset.points().size());
});
for (const auto& [point_index, storage_idx] : valid_point_updates)
{
// 提取优化后的点参数
std::span<const double, 3> point_params(point_params_storage_[storage_idx]);
// 更新数据集中 3D 点坐标
dataset.points()[point_index] = typename Dataset::Traits::Vec3(
point_params[0], point_params[1], point_params[2]
);
}
理解：
- 过滤有效点索引：
- 确保
point_index和storage_idx有效
- 确保
- 提取优化后的坐标：
X j = [ x j , y j , z j ] T X_j = [x_j, y_j, z_j]^T Xj=[xj,yj,zj]T - 写回数据集：
- 将优化后的点坐标更新到原数据集中
3. 数学公式总结
- BA 优化变量：
- 相机 x i ∈ R 10 x_i \in \mathbb{R}^{10} xi∈R10
- 3D 点 X j ∈ R 3 X_j \in \mathbb{R}^3 Xj∈R3
- 残差函数：
r i j ( x i , X j ) = project ( x i , X j ) − ( u i j , v i j ) r_{ij}(x_i, X_j) = \text{project}(x_i, X_j) - (u_{ij}, v_{ij}) rij(xi,Xj)=project(xi,Xj)−(uij,vij) - 优化目标：
min x i , X j ∑ ( i , j ) ∈ observations ∣ r i j ( x i , X j ) ∣ 2 \min_{{x_i}, {X_j}} \sum_{(i,j) \in \text{observations}} | r_{ij}(x_i, X_j) |^2 xi,Xjmin(i,j)∈observations∑∣rij(xi,Xj)∣2 - 优化后更新：
x i new → 更新位姿 + 内参 X j new → 更新 3D 点坐标 x_i^{\text{new}} \rightarrow \text{更新位姿 + 内参} \\ X_j^{\text{new}} \rightarrow \text{更新 3D 点坐标} xinew→更新位姿 + 内参Xjnew→更新 3D 点坐标
4. Pipeline 优化结果示例
| 数据集 | 优化前 Avg Repro Error | 优化后 Avg Repro Error |
|---|---|---|
| Ladybug | 3.859 pixels | 2.042 pixels |
| Trafalgar Square | 8.309 pixels | 2.941 pixels |
理解：
- 重投影误差显著降低，说明优化成功
- 平衡模式（Balanced mode）下的优化效果优良
- 数值单位是像素 (pixels)
1. 窗口外的相机设为常量
// 获取上一窗口中，不在当前窗口中的相机
auto prev_window_cameras = prev_window_cameras_
| std::views::filter([&](int cam_id) {
return !window_cameras.contains(cam_id);
});
// 将这些相机设置为常量
std::ranges::for_each(prev_window_cameras, [&](int cam_id) {
if(auto it = camera_id_to_index_.find(cam_id); it != camera_id_to_index_.end()) {
auto& camera = cameras_[it->second];
problem_->SetParameterBlockConstant(camera.data()); // 相机参数不可优化
camera.is_variable = false; // 标记为非变量
}
});
理解：
- 为什么要设置常量？
- 在窗口化优化中，只优化当前窗口中的相机，其它相机保持固定，保证 BA 问题的局部性
- 避免影响全局相机参数
- 逻辑：
prev_window_cameras_：上一窗口的相机集合window_cameras：当前窗口的相机集合- 不在当前窗口中的相机 → 设置为常量 (
SetParameterBlockConstant)
- 数学意义：
- 对应优化变量
x
i
x_i
xi，若相机不在当前窗口，则
x i is constant ⟹ δ x i = 0 x_i \text{ is constant} \implies \delta x_i = 0 xi is constant⟹δxi=0
- 对应优化变量
x
i
x_i
xi，若相机不在当前窗口，则
2. 当前窗口的相机设为变量
// 当前窗口中新增的相机
auto current_window_cameras = window_cameras
| std::views::filter([&](int cam_id) {
return !prev_window_cameras_.contains(cam_id);
});
// 将这些相机设置为变量
std::ranges::for_each(current_window_cameras, [&](int cam_id) {
if (auto it = camera_id_to_index_.find(cam_id); it != camera_id_to_index_.end()) {
auto& camera = cameras_[it->second];
problem_->SetParameterBlockVariable(camera.data()); // 相机参数可优化
camera.is_variable = true; // 标记为变量
}
});
理解：
- 为什么设置为变量？
- 当前窗口的相机需要参与优化
- 对应 BA 问题中的自由变量 x i x_i xi
- 数学意义：
- 对于当前窗口的相机，允许增量
δ
x
i
\delta x_i
δxi 优化
x i is variable ⟹ δ x i ∈ R 10 x_i \text{ is variable} \implies \delta x_i \in \mathbb{R}^{10} xi is variable⟹δxi∈R10
- 对于当前窗口的相机，允许增量
δ
x
i
\delta x_i
δxi 优化
3. 设置相机参数上下界约束
// 相机参数排列：[qx,qy,qz,qw,tx,ty,tz,fx,k1,k2]
for (int i = 4; i <= 6; ++i) { // tx, ty, tz
problem_->SetParameterLowerBound(camera.data(), i, config_.cam_translation_min);
problem_->SetParameterUpperBound(camera.data(), i, config_.cam_translation_max);
}
// 焦距 f 的约束
problem_->SetParameterLowerBound(camera.data(), 7, config_.focal_min);
problem_->SetParameterUpperBound(camera.data(), 7, config_.focal_max);
// 径向畸变 k1, k2 的约束
problem_->SetParameterLowerBound(camera.data(), 8, config_.k_min);
problem_->SetParameterUpperBound(camera.data(), 8, config_.k_max);
problem_->SetParameterLowerBound(camera.data(), 9, config_.k_min);
problem_->SetParameterUpperBound(camera.data(), 9, config_.k_max);
理解：
- 相机参数顺序：
camera.parameters = [ q x , q y , q z , q w , t x , t y , t z , f , k 1 , k 2 ] \text{camera.parameters} = [q_x, q_y, q_z, q_w, t_x, t_y, t_z, f, k_1, k_2] camera.parameters=[qx,qy,qz,qw,tx,ty,tz,f,k1,k2] - 参数上下界的目的：
- 避免优化出非法参数（如焦距为负，平移过大，旋转异常）
- 保证优化过程稳定
- 数学意义：
- 对每个优化变量
x
i
x_i
xi，设定约束范围
x i lower ≤ x i ≤ x i upper x_i^\text{lower} \le x_i \le x_i^\text{upper} xilower≤xi≤xiupper - 例如：
t x min ≤ t x ≤ t x max , f min ≤ f ≤ f max , k 1 min ≤ k 1 ≤ k 1 max , … t_x^\text{min} \le t_x \le t_x^\text{max}, \quad f_\text{min} \le f \le f_\text{max}, \quad k_1^\text{min} \le k_1 \le k_1^\text{max}, \ldots txmin≤tx≤txmax,fmin≤f≤fmax,k1min≤k1≤k1max,…
- 对每个优化变量
x
i
x_i
xi，设定约束范围
4. 总结
- 窗口化优化的核心：
- 窗口外相机 → 常量，不参与优化
- 当前窗口新增相机 → 变量，参与优化
- 参数约束：
- 平移 ( t x , t y , t z ) (t_x, t_y, t_z) (tx,ty,tz)
- 焦距 f f f
- 径向畸变 ( k 1 , k 2 ) (k_1, k_2) (k1,k2)
- 优化空间限制：
- 避免 BA 优化跳出合理解的范围
- 公式总结：
x i = [ q x q y q z q w t x t y t z f k 1 k 2 ] T x_i = \begin{bmatrix} q_x & q_y & q_z & q_w & t_x & t_y & t_z & f & k_1 & k_2 \end{bmatrix}^T xi=[qxqyqzqwtxtytzfk1k2]T
约束：
t min ≤ t x , t y , t z ≤ t max f min ≤ f ≤ f max k min ≤ k 1 , k 2 ≤ k max t_\text{min} \le t_x, t_y, t_z \le t_\text{max} \\ f_\text{min} \le f \le f_\text{max} \ k_\text{min} \le k_1, k_2 \le k_\text{max} tmin≤tx,ty,tz≤tmaxfmin≤f≤fmax kmin≤k1,k2≤kmax
1. 目标
- 保留点：仅保留在当前窗口中被至少一个相机观测的点。
- 优化条件：
- 支持数 = 1 → 冻结（不优化）
- 支持数 ≥ 2 → 可优化
- 残差添加：为所有保留点的观测添加残差，即使这些观测来自窗口外相机（cross-window observations）。
2. 调整点的 in-window 支持计数
// 记录访问过的点，其支持数可能发生变化
std::unordered_set<int> visited_points;
// 对当前窗口进入的相机
std::ranges::for_each(curr_camera, [&](int cam_id) {
auto cam_obs_it = camera_id_to_obs_indices_.find(cam_id);
std::ranges::for_each(cam_obs_it->second, [&](int obs_idx) {
const auto& obs = observations_[static_cast<size_t>(obs_idx)];
if (auto pt_it = point_id_to_index_.find(obs.point_id); pt_it != point_id_to_index_.end()) {
points_[pt_it->second].in_window_support++; // 支持计数 +1
visited_points.insert(obs.point_id); // 记录该点
}
});
});
理解：
curr_camera→ 当前窗口中新加入的相机集合。camera_id_to_obs_indices_→ 记录每个相机对应的观测索引列表。in_window_support→ 当前点在窗口内被观测的数量。- 逻辑：
- 每个点被新加入窗口的相机观测 → 支持计数 +1
- 将点记录到
visited_points集合，方便后续处理
数学意义：
设 p j p_j pj 为第 j j j 个 3D 点， C in-window ( p j ) C_\text{in-window}(p_j) Cin-window(pj) 为当前窗口中观测该点的相机集合：
in_window_support ∗ j = ∣ C ∗ in-window ( p j ) ∣ \text{in\_window\_support}*j = | C*\text{in-window}(p_j) | in_window_support∗j=∣C∗in-window(pj)∣
当前窗口相机进入时，若观测该点，则：
in_window_support j ← in_window_support j + 1 \text{in\_window\_support}_j \gets \text{in\_window\_support}_j + 1 in_window_supportj←in_window_supportj+1
3. 窗口离开的相机减少支持计数
// 对上一窗口离开的相机
std::ranges::for_each(prev_camera, [&](int cam_id) {
auto cam_obs_it = camera_id_to_obs_indices_.find(cam_id);
std::ranges::for_each(cam_obs_it->second, [&](int obs_idx) {
const auto& obs = observations_[static_cast<size_t>(obs_idx)];
if (auto pt_it = point_id_to_index_.find(obs.point_id); pt_it != point_id_to_index_.end()) {
points_[pt_it->second].in_window_support--; // 支持计数 -1
visited_points.insert(obs.point_id); // 记录该点
}
});
});
理解：
prev_camera→ 上一个窗口中已经离开的相机集合- 每个点被离开窗口的相机观测 → 支持计数 -1
- 记录到
visited_points集合，方便后续更新点是否冻结或优化
数学意义：
in_window_support j ← in_window_support j − 1 \text{in\_window\_support}_j \gets \text{in\_window\_support}_j - 1 in_window_supportj←in_window_supportj−1
如果该点不再被任何窗口内相机观测（支持数 = 0），则该点将被移除优化（冻结）。
4. 核心逻辑总结
- 支持计数更新：
in_window_support j = in_window_support j + ( 新加入窗口相机观测 ) − ( 离开窗口相机观测 ) \text{in\_window\_support}_j = \text{in\_window\_support}_j + (\text{新加入窗口相机观测}) - (\text{离开窗口相机观测}) in_window_supportj=in_window_supportj+(新加入窗口相机观测)−(离开窗口相机观测) - 优化策略：
- 如果 in_window_support j = 0 \text{in\_window\_support}_j = 0 in_window_supportj=0 → 删除点
- 如果 in_window_support j = 1 \text{in\_window\_support}_j = 1 in_window_supportj=1 → 冻结点
- 如果 in_window_support j ≥ 2 \text{in\_window\_support}_j \ge 2 in_window_supportj≥2 → 优化点
- 残差计算：所有保留点的残差均被加入优化，包括来自窗口外相机的观测，这保证了 BA 的全局一致性。
5. 小结
visited_points：只更新被窗口内相机影响的点curr_camera/prev_camera：用于增减支持计数in_window_support：决定点是否冻结或优化- 优化目标：只优化支持足够的点，减少计算量并提高稳定性
1. 保留与冻结点（Points: Keep/Freeze）
if (!point.is_in_problem)
problem_->AddParameterBlock(point.data(), 3); // 将点加入优化问题，每个点有3个参数（x, y, z）
point.is_in_problem = true; // 标记该点已经在优化问题中
// 设置点坐标的参数上下限（可选）
if (config_.use_parameter_bounds)
for (int i = 0; i < 3; ++i) {
problem_->SetParameterLowerBound(point.data(), i, config_.point_min);
problem_->SetParameterUpperBound(point.data(), i, config_.point_max);
}
// 冻结或激活优化
if (point.in_window_support == 1 && config_.freeze_weak_points) {
problem_->SetParameterBlockConstant(point.data()); // 只被一个窗口相机观测 → 冻结
point.is_variable = false; // 标记为不可优化
} else {
problem_->SetParameterBlockVariable(point.data()); // 支持 ≥ 2 → 优化
point.is_variable = true; // 标记为可优化
}
理解：
- 加入优化问题：如果该点还没有加入优化问题，就调用
AddParameterBlock。 - 参数范围：可选地限制点坐标在
[point_min, point_max]范围内，提高优化稳定性。 - 冻结弱点：如果点只被 1 个窗口相机观测，且配置允许冻结弱点，则不优化该点；否则参与优化。
数学意义：
- 对于每个点
p
j
p_j
pj，其在窗口内的支持数为
s
j
s_j
sj：
- s j = 0 s_j = 0 sj=0 → 不加入优化（移除点）
- s j = 1 s_j = 1 sj=1 → 冻结点（ p j p_j pj 不作为变量）
- s j ≥ 2 s_j \ge 2 sj≥2 → 优化点（ p j p_j pj 作为变量）
- 对于冻结点：
p j 固定 , 优化问题中不包含该点参数 p_j \text{ 固定}, \quad \text{优化问题中不包含该点参数} pj 固定,优化问题中不包含该点参数 - 对于可优化点：
p j ∈ 优化变量集合 , 残差函数 r(x) 对该点求导计算 Jacobian p_j \in \text{优化变量集合}, \quad \text{残差函数 r(x) 对该点求导计算 Jacobian} pj∈优化变量集合,残差函数 r(x) 对该点求导计算 Jacobian
2. Schur 分组（Parameter Ordering for Schur Complement）
if (!config_.use_schur_ordering) return;
parameter_ordering_ = std::make_shared<ceres::ParameterBlockOrdering>();
// Group 0: Points (通过 Schur 消元消去)
for (auto& point : points)
if (point.is_variable && point.is_in_problem)
parameter_ordering_->AddElementToGroup(point.data(), 0);
// Group 1: Cameras (在约简系统中保留)
for (auto& camera : cameras)
if (camera.is_variable && camera.is_in_problem)
parameter_ordering_->AddElementToGroup(camera.data(), 1);
solver_options_.linear_solver_ordering = parameter_ordering_;
理解：
- Schur 补分组：
- Ceres 支持 Schur 补加速 BA（Bundle Adjustment），通过消元点参数，只解相机参数。
- Group 0 → 点参数（Point blocks）
- Group 1 → 相机参数（Camera blocks）
- 逻辑：
- Group 0（Points）：优先消元，减小线性系统规模
- Group 1（Cameras）：保留在约简系统中，最终求解
- 优化性能：
- 即使 Ceres 会自动分组，手动分组保证 BA 在稀疏系统中性能最优。
3. 数学意义：Schur 补
设 BA 的线性系统为：
[
J
p
T
J
p
J
p
T
J
c
J
c
T
J
p
J
c
T
J
c
]
[
δ
p
δ
c
]
=
−
[
J
p
T
r
J
c
T
r
]
\begin{bmatrix} J_p^T J_p & J_p^T J_c \\ J_c^T J_p & J_c^T J_c \end{bmatrix} \begin{bmatrix} \delta_p \\ \delta_c \end{bmatrix} = -\begin{bmatrix} J_p^T r \\ J_c^T r \end{bmatrix}
[JpTJpJcTJpJpTJcJcTJc][δpδc]=−[JpTrJcTr]
- p p p → 点参数（Point blocks）
-
c
c
c → 相机参数（Camera blocks）
通过 Schur 补消元点参数 δ p \delta_p δp：
( J c T J c − J c T J p ( J p T J p ) − 1 J p T J c ) δ c = − ( J c T r − J c T J p ( J p T J p ) − 1 J p T r ) (J_c^T J_c - J_c^T J_p (J_p^T J_p)^{-1} J_p^T J_c) \delta_c = -(J_c^T r - J_c^T J_p (J_p^T J_p)^{-1} J_p^T r) (JcTJc−JcTJp(JpTJp)−1JpTJc)δc=−(JcTr−JcTJp(JpTJp)−1JpTr) - 这样只解 δ c \delta_c δc（相机更新），再回代得到 δ p \delta_p δp（点更新）。
- 手动分组确保 Group 0 是点，Group 1 是相机 → 符合 Schur 消元顺序。
4. 小结
- 点参数加入与冻结逻辑：
- 支持数 = 0 → 移除
- 支持数 = 1 → 冻结
- 支持数 ≥ 2 → 优化
- Schur 补分组：
- Group 0 → 点参数，先消元
- Group 1 → 相机参数，最终求解
- 提升 BA 求解效率
- 数学公式：
- 通过 Schur 补，将大系统：
[ J p T J p J p T J c J c T J p J c T J c ] [ δ p δ c ] = − [ J p T r J c T r ] \begin{bmatrix} J_p^T J_p & J_p^T J_c \\ J_c^T J_p & J_c^T J_c \end{bmatrix} \begin{bmatrix} \delta_p \\ \delta_c \end{bmatrix} = -\begin{bmatrix} J_p^T r \\ J_c^T r \end{bmatrix} [JpTJpJcTJpJpTJcJcTJc][δpδc]=−[JpTrJcTr]
转化为只含相机参数 δ c \delta_c δc 的小系统，降低计算复杂度。
- 通过 Schur 补，将大系统：
1. 内循环迭代（Inner Iterations）
if (!config_.use_inner_iterations) return;
inner_iteration_ordering = std::make_shared<ceres::ParameterBlockOrdering>();
// Group 0: Points（先优化点）
// 在内循环中固定相机，只优化点参数
for (auto& point : points)
if (point.is_variable && problem_->HasParameterBlock(point.data()))
inner_iteration_ordering->AddElementToGroup(point.data(), 0);
// Group 1: Cameras（后优化相机）
// 固定点参数，只优化相机参数
for (auto& camera : cameras_)
if (camera.is_variable && problem_->HasParameterBlock(camera.data()))
inner_iteration_ordering_->AddElementToGroup(camera.data(), 1);
solver_options_.inner_iteration_ordering = inner_iteration_ordering_;
理解和注释：
- 目的：
- 将大问题拆分为两个子问题，降低每次优化的复杂度。
- Group 0 → 优化点参数，同时固定相机。
- Group 1 → 优化相机参数，同时固定点。
- 优点：
- 每次优化问题规模小，数值更稳定。
- 对高维稀疏问题尤其有利。
- 缺点：
- 可能增加迭代次数。
- 不保证结果比联合优化更好。
- 数学描述：
- 假设原优化目标是残差平方和：
min c , p ∑ i , j ∣ r i j ( c i , p j ) ∣ 2 \min_{c, p} \sum_{i,j} | r_{ij}(c_i, p_j) |^2 c,pmini,j∑∣rij(ci,pj)∣2 - 内循环迭代拆分为两步：
- 固定
c
i
c_i
ci（相机），优化
p
j
p_j
pj（点）：
min p j ∑ i , j ∣ r i j ( c i fixed , p j ) ∣ 2 \min_{p_j} \sum_{i,j} | r_{ij}(c_i^\text{fixed}, p_j) |^2 pjmini,j∑∣rij(cifixed,pj)∣2
- 固定
c
i
c_i
ci（相机），优化
p
j
p_j
pj（点）：
- 固定
p
j
p_j
pj，优化
c
i
c_i
ci：
min c i ∑ i , j ∣ r i j ( c i , p j fixed ) ∣ 2 \min_{c_i} \sum_{i,j} | r_{ij}(c_i, p_j^\text{fixed}) |^2 cimini,j∑∣rij(ci,pjfixed)∣2
- 迭代进行，直至收敛或达到最大迭代次数。
2. 损失函数（Loss Functions）
class AdaptiveHuberLoss : public ceres::LossFunction
{
public:
explicit AdaptiveHuberLoss(double initial_delta = 1.0)
: delta(initial_delta) {}
void Evaluate(double s, double rho[3]) const override
{
// s 是残差平方： s = r^2
// rho[0] = 损失函数值，rho[1] = 一阶导，rho[2] = 二阶导
if (s <= delta*delta) {
rho[0] = s;
rho[1] = 1.0;
rho[2] = 0.0;
} else {
double sqrt_s = std::sqrt(s);
rho[0] = 2*delta*sqrt_s - delta*delta; // Huber 损失公式
rho[1] = delta / sqrt_s;
rho[2] = -0.5 * delta / (s*sqrt_s);
}
}
private:
double delta;
};
理解和注释：
- Huber 损失：
- 小残差
∣
r
∣
≤
δ
|r| \le \delta
∣r∣≤δ → 使用平方损失（Quadratic Loss）
ρ ( r ) = r 2 \rho(r) = r^2 ρ(r)=r2 - 大残差
∣
r
∣
>
δ
|r| > \delta
∣r∣>δ → 使用线性损失（Linear Loss）
ρ ( r ) = 2 δ ∣ r ∣ − δ 2 \rho(r) = 2 \delta |r| - \delta^2 ρ(r)=2δ∣r∣−δ2
- 小残差
∣
r
∣
≤
δ
|r| \le \delta
∣r∣≤δ → 使用平方损失（Quadratic Loss）
- Cauchy 损失（未给出代码，但常用）：
- 对大残差更鲁棒，但收敛可能更慢
- 数学形式：
ρ ( r ) = c 2 log ( 1 + r 2 c 2 ) \rho(r) = c^2 \log\left(1 + \frac{r^2}{c^2}\right) ρ(r)=c2log(1+c2r2)
- AdaptiveHuberLoss 类：
- 封装 Huber 损失，可设置初始阈值
delta Evaluate函数根据残差平方s = r^2输出：rho[0]→ 损失值rho[1]→ 一阶导（权重因子）rho[2]→ 二阶导
- 封装 Huber 损失，可设置初始阈值
- 数学意义：
- Huber 损失是一种 鲁棒损失，可以在残差很大的异常观测上减少梯度影响：
ρ ( r ) = { r 2 ∣ r ∣ ≤ δ 2 δ ∣ r ∣ − δ 2 ∣ r ∣ > δ \rho(r) = \begin{cases} r^2 & |r| \le \delta \\ 2 \delta |r| - \delta^2 & |r| > \delta \end{cases} ρ(r)={r22δ∣r∣−δ2∣r∣≤δ∣r∣>δ - 相比于普通平方误差，Huber 可以防止异常值对优化结果的过度影响。
✓ 总结
- Inner Iterations：
- 将 BA 拆成 点优化 + 相机优化 两个子问题
- 优势：稳定、稀疏矩阵处理更快
- 劣势：可能需要更多迭代，不保证全局最优
- 损失函数：
- Huber：小残差平方损失，大残差线性损失
- Cauchy：对大残差更鲁棒，但慢
- 自定义损失可根据需要调整
- 数学公式：
- 内循环优化公式：
min p j ∑ i , j ∣ r i j ( c i fixed , p j ) ∣ 2 then min c i ∑ i , j ∣ r i j ( c i , p j fixed ) ∣ 2 \min_{p_j} \sum_{i,j} | r_{ij}(c_i^\text{fixed}, p_j) |^2 \quad \text{then} \quad \min_{c_i} \sum_{i,j} | r_{ij}(c_i, p_j^\text{fixed}) |^2 pjmini,j∑∣rij(cifixed,pj)∣2thencimini,j∑∣rij(ci,pjfixed)∣2
- 内循环优化公式：
- Huber 损失公式：
ρ ( r ) = { r 2 ∣ r ∣ ≤ δ 2 δ ∣ r ∣ − δ 2 ∣ r ∣ > δ \rho(r) = \begin{cases} r^2 & |r| \le \delta \\ 2 \delta |r| - \delta^2 & |r| > \delta \end{cases} ρ(r)={r22δ∣r∣−δ2∣r∣≤δ∣r∣>δ
开源鸿蒙跨平台开发社区汇聚开发者与厂商，共建“一次开发，多端部署”的开源生态，致力于降低跨端开发门槛，推动万物智联创新。
更多推荐
15
0- 0
已为社区贡献17条内容
所有评论(0)