Eigen使用笔记:如何进行矩阵和标量加减、类似numpy where等方法

本文详细介绍Eigen库中数据结构转换、数值运算、条件筛选等高级功能,包括std::vector与Eigen数据结构互转、矩阵标量运算、求极值、部分块截取、条件筛选、行列操作、绝对值计算等内容。

1.std::vector转Eigen的数据结构

a.通过Eigen::Map转VectorXf,长度不用编译时期确定
std::vector<float> test_vec1 = {1, 2, 3};
Eigen::Map<Eigen::VectorXf> test_mat1(test_vec1.data(), test_vec1.size());
std::cout << "test_mat1 before:" << test_mat1 << std::endl;
test_vec1[1]=100;
std::cout << "test_mat1 after:" << test_mat1 << std::endl;

output:

test_mat1 before:1
2
3
test_mat1 after:  1
100
  3

可以看到这种方式并不是深拷贝,这点需要特别注意.

b.转Matrix,长度需要编译时期确定
std::vector<float> test_vec2 = {1, 2, 3};
Eigen::Matrix<float, 1, 3> test_mat2(test_vec2.data());
std::cout << "test_mat2 before:" << test_mat2 << std::endl;
test_vec2[1]=100;
std::cout << "test_mat2 after:" << test_mat2 << std::endl;

output:

test_mat2 before:1 2 3
test_mat2 after:1 2 3

多维情况:

std::vector<float> test_vec3 = {1, 2, 3, 4};
Eigen::Map<Eigen::MatrixXf> test_mat3(test_vec3.data(), 2, 2);
std::cout << "test_mat3 before:" << test_mat3 << std::endl;
test_vec3[1]=100;
std::cout << "test_mat3 after:" << test_mat3 << std::endl;

output:

test_mat3 before:1 3
2 4
test_mat3 after:  1   3
100   4

2.数值运算

矩阵和标量的加减运算

Eigen本身是不支持矩阵类和标量的加减的,因此需要转换.同时矩阵类的加减法必须是两个同类型同维度的矩阵类相加减.
a.矩阵减

Eigen::Matrix<float, 2, 3> data3;
data3 << 0., 1., 2., 3., 4., 5.;
std::cout << "data3:" << data3 << std::endl;
float coeff = 0.1;
auto data4 = (data3 - Eigen::MatrixXf::Ones(data3.rows(), data3.cols()) * coeff);
std::cout << "data4:" << data4 << std::endl;

output:

data3:0 1 2
3 4 5
data4:-0.1  0.9  1.9 
 2.9  3.9  4.9

b.向量减

Eigen::VectorXf data5(6);
data5 << 0., 1., 2., 3., 4.,5.;
std::cout << "data5:" << data5 << std::endl;
coeff = 0.5;
auto data6 = (data5 + Eigen::VectorXf::Ones(data5.size()) * coeff);
std::cout << "data6:" << data6 << std::endl;

output:

data5:0
1
2
3
4
5
data6:0.5
1.5
2.5
3.5
4.5
5.5

3.最大值和最小值

maxCoeff不加参数时只获取最大值,加参数可以获取最大值坐标.(minCoeff同理)

auto max = data3.maxCoeff();
std::cout << "max:" << max << std::endl;
int max_row, max_col;
max = data3.maxCoeff(&max_row, &max_col);
std::cout << "max_row:" << max_row << std::endl;
std::cout << "max_col:" << max_col << std::endl;

auto min = data3.minCoeff();
std::cout << "min:" << min << std::endl;
int min_row, min_col;
min = data3.minCoeff(&min_row, &min_col);
std::cout << "min_row:" << min_row << std::endl;
std::cout << "min_col:" << min_col << std::endl;

output

max:5
max_row:1
max_col:2
min:0
min_row:0
min_col:0

4.截取部分块

提取部分数据,类似切片的功能.需要使用block()方法.具体使用有两种方式:
matrix.block(i,j,p,q)和matrix.block<p,q>(i,j),前者用于大小不固定的,可以动态提取大小.后者必须是固定的参数.其中,(i,j)是起始位置,(p,q)是提取的大小.

Eigen::Matrix<float, 2, 4> data7;
data7 << 0., 1., 2., 3., 4., 5., 6., 7.;
std::cout << "data7:" << data7 << std::endl;
// 起始(0, 1), 大小(2, 2)
auto data8 = data7.block(0, 1, 2, 2);
std::cout << "data8:" << data8 << std::endl;

output:

data7:0 1 2 3
4 5 6 7
data8:1 2
5 6

block()也可以用于对矩阵进行部分赋值

5.类似numpy.where的功能,添加条件筛选

判断矩阵中的元素是否都大于某个值,通过.array()来实现.这里返回的ret为和data7同样size的矩阵,每个元素为1或者0,表示对应的data7的元素是否满足条件.
另外,all()返回是否全部元素都满足条件,any()返回是否存在一个满足条件的元素.count()返回满足条件元素的个数.

auto threshold = 1.5;
auto ret = (data7.array() > threshold);
std::cout << "ret:" << ret << std::endl;
std::cout << "ret.all():" << ret.all() << std::endl;
std::cout << "ret.any():" << ret.any() << std::endl;
std::cout << "ret.count():" << ret.count() << std::endl;

output:

ret:0 0 1 1
1 1 1 1
ret.all():0
ret.any():1
ret.count():6

6.按照行或者列,求平均值或者求和

rowwise()表示按照行,而colwise()表示按照列.

// 按行或者列求平均
auto mean1 = data7.rowwise().mean();
auto mean2 = data7.colwise().mean();
std::cout << "mean1:" << mean1 << std::endl;
std::cout << "mean2:" << mean2 << std::endl;
// 按行或者列求和
auto sum1 = data7.rowwise().sum();
auto sum2 = data7.colwise().sum();
std::cout << "sum1:" << sum1 << std::endl;
std::cout << "sum2:" << sum2 << std::endl;

output:

mean1:1.5
5.5
mean2:2 3 4 5
sum1: 6
22
sum2: 4  6  8 10

7.绝对值

cwiseAbs()返回绝对值,cwiseAbs2()返回绝对值的平方

Eigen::Matrix<float, 2, 4> data9;
data9 << -1., -2., -3., 4., -5., -6., -7., 8;
auto data9_abs = data9.cwiseAbs();
auto data9_abs2 = data9.cwiseAbs2();
std::cout << "data9:" << data9 << std::endl;
std::cout << "data9_abs:" << data9_abs << std::endl;
std::cout << "data9_abs2:" << data9_abs2 << std::endl;

output:

data9:-1 -2 -3  4
-5 -6 -7  8
data9_abs:1 2 3 4 5 6 7 8 
data9_abs2: 1  4  9 16
25 36 49 64
<think> 我们之前已经讨论过使用Eigen::QuaterniondEigen::Translation进行点坐标转换的方法。根据用户当前的问题,他希望仅使用这两个类(而不使用Transform或Isometry等组合变换对象)来进行点坐标转换。因此,我们需要提供直接使用四元平移向量的方法。 核心公式仍然是: $$ \mathbf{p}' = \mathbf{q} \cdot \mathbf{p} + \mathbf{t} $$ 其中: $\mathbf{q}$ 是旋转四元(注意:必须是单位四元) $\mathbf{p}$ 是原始点(Eigen::Vector3d) $\mathbf{t}$ 是平移向量(Eigen::Translation3d的vector部分或直接使用Eigen::Vector3d) $\mathbf{p}'$ 是变换后的点 具体步骤: 1. 使用四元旋转点:旋转后的点 = q * p (注意:四元乘法需要将点表示为虚四元) 2. 将旋转后的点加上平移向量 注意:Eigen::Quaterniond重载了乘以Eigen::Vector3d的运算符,因此可以直接使用q * p来计算旋转。 另外,Eigen::Translation3d对象可以通过.vector()方法或.vec()方法获取其平移向量(Eigen::Vector3d类型),也可以直接使用Eigen::Vector3d表示平移。 因此,有两种方式: 方式1:直接使用Eigen::Vector3d表示平移 方式2:使用Eigen::Translation3d,然后取其向量部分 下面给出代码示例: ```cpp #include <Eigen/Geometry> #include <iostream> int main() { // 1. 定义旋转(绕Z轴90度) Eigen::Quaterniond q = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ())); q.normalize(); // 确保单位四元 // 2. 定义平移:使用Eigen::Translation3d Eigen::Translation3d translation(2.0, 0.0, 0.0); // 3. 原始点 Eigen::Vector3d p(1.0, 0.0, 0.0); // 4. 进行变换:先旋转,后平移 Eigen::Vector3d transformed_point = q * p + translation.vec(); // 5. 输出结果 std::cout << "Transformed point (using Translation3d): " << transformed_point.transpose() << std::endl; // 也可以直接使用Eigen::Vector3d表示平移 Eigen::Vector3d t_vec(2.0, 0.0, 0.0); transformed_point = q * p + t_vec; std::cout << "Transformed point (using Vector3d): " << transformed_point.transpose() << std::endl; return 0; } ``` 输出结果: ``` Transformed point (using Translation3d): 2 1 0 Transformed point (using Vector3d): 2 1 0 ``` 解释: 原始点(1,0,0)绕Z轴旋转90度后变成(0,1,0),然后加上平移(2,0,0)得到(2,1,0)。 注意事项: 1. 四元乘法顺序:Eigen中四元乘以向量表示旋转该向量,顺序为:q * p(等价于q * p * q^{-1},其中p被当作虚四元)。 2. 平移加法:平移向量直接加到旋转后的点上。 3. 变换顺序:这里明确是先旋转后平移。如果变换顺序不同(例如先平移后旋转),则公式应改为:q * (p + t) 或等价的其他形式。但通常刚体变换是先旋转后平移。 如果变换顺序需要改变(比如先平移后旋转),则: transformed_point = q * (p + t_vec); // 先平移,后旋转 或者学上等价于:q * p + q * t_vec (注意这里的平移量是旋转后的平移量) 因此,根据实际变换顺序调整表达式。 引用说明: 在Sophus库中,SE3同样由四元平移向量组成,其据存储顺序为先旋转(四元的四个分量)后平移(三个分量)[^1]。 不过,我们这里直接使用Eigen的四元平移,不需要依赖Sophus。 相关问题: 1. 如果不进行四元归一化会有什么后果? 2. 如何将多个旋转平移变换组合成一个变换? 3. 在点云处理中,如何高效地对大量点进行四元平移变换? 4. 四元旋转点使用旋转矩阵旋转点,在效率上有何差异? 5. 如何从Eigen::Transform对象中提取四元平移向量? </think> ### 仅使用 `Eigen::Quaterniond` `Eigen::Translation` 的点坐标转换方法Eigen 中,**无需组合为 Transform 对象**,可直接使用四元平移向量实现点坐标转换。核心公式为: $$ \mathbf{p}' = \mathbf{q} \cdot \mathbf{p} + \mathbf{t} $$ 其中: - $\mathbf{p}$ 是原始点坐标 (`Eigen::Vector3d`) - $\mathbf{q}$ 是单位旋转四元 (`Eigen::Quaterniond`) - $\mathbf{t}$ 是平移向量 (`Eigen::Translation3d::vector()`) - $\mathbf{p}'$ 是变换后的坐标 #### 完整代码实现 ```cpp #include <Eigen/Geometry> #include <iostream> int main() { // 1. 初始化旋转四元(绕Z轴旋转90度) Eigen::Quaterniond q(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ())); q.normalize(); // 关键:必须归一化[^1] // 2. 初始化平移(X方向平移2单位) Eigen::Translation3d t(2.0, 0.0, 0.0); // 3. 原始点坐标 Eigen::Vector3d point(1.0, 0.0, 0.0); // 4. 直接计算变换:旋转 + 平移 Eigen::Vector3d transformed_point = q * point + t.vec(); // 等价写法:q * point + t.vector() // 验证:原始点(1,0,0) -> 旋转后(0,1,0) -> 平移后(2,1,0) std::cout << "Transformed point: " << transformed_point.transpose() << std::endl; return 0; } ``` #### 关键操作说明 1. **旋转操作** ```cpp // 四元旋转点坐标(直接使用重载的*运算符) Eigen::Vector3d rotated = q * point; ``` 2. **平移操作提取** ```cpp // 从Translation3d获取平移向量 Eigen::Vector3d trans_vec = t.vec(); // 或 t.vector() ``` 3. **组合变换** ```cpp // 先旋转后平移(标准刚体变换) transformed_point = q * point + t.vec(); ``` #### 变换顺序控制 - **先旋转后平移**(标准刚体变换): ```cpp result = q * point + t.vec(); ``` - **先平移后旋转**(较少使用): ```cpp result = q * (point + t.vec()); ``` #### 性能优化建议 1. **批量点转换**:使用 Eigen矩阵运算 ```cpp // points: 3xN矩阵 (每列一个点) Eigen::MatrixXd points = ...; Eigen::MatrixXd transformed = (q * points).colwise() + t.vec(); ``` 2. **避免临时对象**:链式表达式优化 ```cpp // 编译器会优化临时对象 result = q * point + t.vec(); ``` #### 应用场景示例 ```cpp // 相机位姿应用:将局部坐标转换为世界坐标 Eigen::Quaterniond cam_rotation = ...; Eigen::Translation3d cam_translation = ...; Eigen::Vector3d world_point = cam_rotation * local_point + cam_translation.vec(); ``` > **关键注意事项** > 1. 四元 **必须调用 `.normalize()`** 确保单位长度,否则会引入缩放[^1] > 2. 平移向量通过 `.vec()` 或 `.vector()` 获取,直接相加 > 3. 变换顺序直接影响结果(刚体变换通常是先旋转后平移)
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值