Eigen中条件与归约操作详解和实际场景示例

在 Eigen（C++ 矩阵库）中，**条件操作（Conditionals）和归约操作（Reductions）**是进行矩阵元素筛选、统计、逻辑判断等非常重要的工具，广泛应用于数值计算、机器学习、SLAM 等场景。

---

一句话概括

• 条件操作：类似于 if，可以筛选、替换、逻辑比较矩阵元素（如 .select()、逻辑比较符号）
• 归约操作：将整个矩阵或数组“缩减”为标量（如 .sum()、.mean()、.minCoeff()）

---

1. 条件操作（Conditionals）

Eigen 的 Array 类支持逐元素的条件判断与筛选。

示例：逐元素替换（类似 NumPy 的 np.where()）

#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;

int main() {
    ArrayXXf A(2, 2);
    A << 1.0, -2.0,
         3.0, -4.0;

    // 替换负数为0，保留正数
    ArrayXXf B = A.select(A > 0, 0);  // 如果 A(i,j) > 0，用 A(i,j)，否则用 0

    std::cout << "A = \n" << A << "\n\n";
    std::cout << "B = \n" << B << "\n";  // B 只有正数
}

输出：

A =
 1 -2
 3 -4

B =
 1  0
 3  0

---

通用语法

Array.select(ifTrue, ifFalse);

每个位置满足条件时使用 ifTrue 的值，否则使用 ifFalse。

---

常见条件比较符

符号	说明
==	相等
!=	不等
> <	大于、小于
>= <=	大于等于等

结果是一个 布尔 Array（数组），可以用于 .select()。

---

2. 归约操作（Reductions）

用于将矩阵或数组“压缩”为一个标量（最大值、和、均值等）。

示例：基本归约函数

#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;

int main() {
    Matrix3f M;
    M << 1, 2, 3,
         4, 5, 6,
         7, 8, 9;

    std::cout << "sum = " << M.sum() << std::endl;           // 45
    std::cout << "mean = " << M.mean() << std::endl;         // 5
    std::cout << "min = " << M.minCoeff() << std::endl;      // 1
    std::cout << "max = " << M.maxCoeff() << std::endl;      // 9
    std::cout << "trace = " << M.trace() << std::endl;       // 15 (对角线和)
}

---

归约函数总览

函数	含义
.sum()	所有元素之和
.mean()	所有元素平均值
.minCoeff()	最小值
.maxCoeff()	最大值
.trace()	主对角线元素之和
.prod()	所有元素乘积
.count()	true 元素的个数（结合条件）

---

3. 条件 + 归约组合：计数、筛选统计

示例：统计矩阵中大于 3 的元素个数

ArrayXXf A(2, 3);
A << 1, 2, 3,
     4, 5, 6;

int count = (A > 3).count();  // 返回 true 的个数
std::cout << "元素大于 3 的个数: " << count << std::endl;  // 输出 3

---

4. 获取最大/最小值位置

MatrixXi M(2, 2);
M << 1, 50,
     3, 4;

int row, col;
int maxVal = M.maxCoeff(&row, &col);

std::cout << "最大值是 " << maxVal << "，位置在 (" << row << ", " << col << ")\n";

---

5. 条件过滤后再归约（稍复杂）

ArrayXXf A(3, 3);
A << 1, 2, -3,
     4, -5, 6,
     -7, 8, 9;

// 将负数替换为 0，然后求和
float filtered_sum = A.select(A > 0, 0).sum();
std::cout << "去除负数后的总和：" << filtered_sum << std::endl;

---

实战场景示例

下面来深入说明在 实际工作中如何使用 Eigen 的 select() 和 reduction 操作，尤其是用于以下情况：

• 鲁棒优化中的 outlier 处理（比如 Huber / Cauchy）
• 残差项约束剔除
• 动态点（moving points）筛除
• 特征过滤与加权处理

---

场景 1：鲁棒核函数中使用 select() 替代分段函数

在 SLAM 后端优化（比如 BA、Pose Graph）中，为了处理 outlier（外点），我们经常使用 Huber 或 Cauchy 核函数来降低大残差的影响。

---

示例：Huber 核函数（伪代码用 Eigen 实现）

定义：

$$\rho (r)=\{\begin{array}{ll}\frac{1}{2}{r}^2& \text{if }|r|\le \delta \\ \delta (|r|-\frac{1}{2}\delta )& \text{otherwise}\end{array}$$

#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;

int main() {
    ArrayXf residuals(6);
    residuals << 0.5, 0.7, 1.0, 1.5, 2.0, 5.0;  // 残差（绝对值）

    float delta = 1.0;

    ArrayXf huber_cost = (residuals.abs() <= delta)
        .select(0.5 * residuals.square(),           // 内点代价
                delta * (residuals.abs() - 0.5 * delta)); // 外点代价

    std::cout << "Huber 代价:\n" << huber_cost.transpose() << "\n";
    std::cout << "总损失: " << huber_cost.sum() << std::endl;
}

---

说明：

• select(cond, true_val, false_val) 实现条件分段函数
• residuals.square() 是内点二次项
• 外点处理是线性增长，限制了极大值影响

---

场景 2：动态点剔除（前端滤除运动物体）

在建图或激光 SLAM 中，动态物体造成的点云误差很大，通常通过阈值筛选掉。

---

示例：移除 Z 方向跳变过大的点（点云滤波）

ArrayXf z_vals(6);  // 点云 z 坐标
z_vals << 1.0, 1.1, 1.0, 3.5, 1.2, 10.0;  // 第4,6个点是动态点或噪声

float z_thresh = 2.0;

ArrayXf mask = (z_vals < z_thresh);  // true = 静态点

std::cout << "有效点数：" << mask.count() << std::endl;

可以进一步用 select() 构造新的滤波数组：

ArrayXf filtered_z = mask.select(z_vals, 0.0f);  // 动态点置为0或ignore

---

场景 3：BA 残差大于阈值的观测被剔除

在视觉 SLAM 的 Bundle Adjustment 优化中，残差大的观测可能是误匹配，需要剔除：

ArrayXf reproj_errors(5);  // 像素误差
reproj_errors << 0.5, 1.2, 0.8, 5.0, 0.9;

float th = 2.0;

Array<bool, Dynamic, 1> inliers = (reproj_errors < th);  // true 为内点

int num_inliers = inliers.count();
std::cout << "有效观测数 = " << num_inliers << std::endl;

---

场景 4：特征权重归一化（比如加权 ICP）

假设我们为每个点赋予一个权重（可由曲率、强度、置信度估计），使用 .sum() 实现归一化：

ArrayXf weights(4);
weights << 0.1, 0.2, 0.4, 0.3;

ArrayXf normalized = weights / weights.sum();
std::cout << "归一化权重 = " << normalized.transpose() << std::endl;

---

小结

应用场景	使用方式	示例函数
鲁棒核函数	条件残差替换	select(abs(r)<delta, ...)
动态点剔除	点云过滤	(z < z_thresh).count()
观测剔除（BA）	剔除大误差观测	(error < threshold).count()
权重归一化	用于残差加权 / ICP	.sum()、.select()
最大残差定位	查找优化失败点	.maxCoeff(&i, &j)

---

小结

类型	方法	示例
条件	.select()	A.select(A > 0, 0)
归约	.sum(), .mean()	M.sum()，M.minCoeff()
统计	.count()	(A > 0).count()
位置	.maxCoeff(&i, &j)	获取最大值及其坐标
组合	.select().sum()	条件过滤后求和

---