文章目录
ubuntu下默认安装路径
/usr/include/eigen3
Eigen设计理念
Eigen使用奇异递归模板模式（curiously recurring template pattern，CRTP）来实现多态，把派生类作为基类的模板参数，避免虚函数庞大的开销影响性能。
例如：
Matrix继承自MatrixBase<Matrix>Array继承自ArrayBase<Array>
template <typename Der>
void generateRandomPointCloud(
Eigen::MatrixBase<Derived>& mat, const size_t N, const size_t dim,
const typename Der::Scalar max_range = 10)
{
std::cout << "Generating " << N << " random points...";
mat.resize(N, dim);
for (size_t i = 0; i < N; i++)
for (size_t d = 0; d < dim; d++)
mat(i, d) =
max_range * (rand() % 1000) / typename Derived::Scalar(1000);
std::cout << "done\n";
}
Eigen矩阵运算
头文件
#include<eigen3/Eigen/Eigen>//所有头文件
#include<eigen3/Eigen/Core>//Eigen核心部分
#include<eigen3/Eigen/Dense>//稠密矩阵的代数运算(逆、特征值等)
using namespace Eigen;//以下所有代码均在Eigen命名空间
矩阵定义
using namespace Eigen;
Eigen::Matrix<double, 3, 3> A; // Fixed rows and cols. Same as Matrix3d.
Matrix<double, 3, Dynamic> B; // Fixed rows, dynamic cols.
Matrix<double, Dynamic, Dynamic> C; // Full dynamic. Same as MatrixXd.
//Matrix创建的矩阵默认是按列存储，Eigen在处理按列存储的矩阵时会更加高效。如果想修改可以在创建矩阵的时候加入参数
Eigen::Matrix<double, 3, 3, ColMajor> E; // Column major; default is column-major.
Eigen::Matrix<double, 3, 3, RowMajor> E; // Row major; default is column-major.
Matrix3f P, Q, R; // 3x3 float matrix.
Vector3f x, y, z; // 3x1 float matrix.
RowVector3f a, b, c; // 1x3 float matrix.
VectorXd v; // Dynamic column vector of doubles
// Eigen // Matlab // comments
x.size() // length(x) // vector size
C.rows() // size(C,1) // number of rows
C.cols() // size(C,2) // number of columns
x(i) // x(i+1) // Matlab is 1-based
C(i,j) // C(i+1,j+1) //
单点元素访问
在矩阵的访问中，行索引总是作为第一个参数，Eigen中矩阵、数组、向量的下标都是从0开始。矩阵元素的访问可以通过”()”操作符完成。例如m(2, 3)既是获取矩阵m的第2行第3列元素。
针对向量还提供”[]”操作符，注意矩阵则不可如此使用。
矩阵赋值与改变维度
A.resize(4, 4); // Runtime error if assertions are on.
B.resize(4, 9); // Runtime error if assertions are on.
A.resize(3, 3); // Ok; size didn't change.
B.resize(3, 9); // Ok; only dynamic cols changed.
A << 1, 2, 3, // Initialize A. The elements can also be
4, 5, 6, // matrices, which are stacked along cols
7, 8, 9; // and then the rows are stacked.
B << A, A, A; // B is three horizontally stacked A's.
A.fill(10); // Fill A with all 10's.
特殊矩阵生成
// Eigen // Matlab
MatrixXd::Identity(rows,cols) // eye(rows,cols)
C.setIdentity(rows,cols) // C = eye(rows,cols)
MatrixXd::Zero(rows,cols) // zeros(rows,cols)
C.setZero(rows,cols) // C = ones(rows,cols)
MatrixXd::Ones(rows,cols) // ones(rows,cols)
C.setOnes(rows,cols) // C = ones(rows,cols)
MatrixXd::Random(rows,cols) // rand(rows,cols)*2-1 // MatrixXd::Random returns uniform random numbers in (-1, 1).
C.setRandom(rows,cols) // C = rand(rows,cols)*2-1
VectorXd::LinSpaced(size,low,high) // linspace(low,high,size)'
v.setLinSpaced(size,low,high) // v = linspace(low,high,size)'
矩阵乘法，数乘
A*B;//矩阵乘法
A*10;//数乘
矩阵行列式
A.determinant();
矩阵的迹
A.trace()
矩阵各元素求和
A.sum()
矩阵的逆
A.inverse();
A.colPivHouseholderQr().inverse() // 实际测试比A.inverse()更准确
矩阵点乘、叉乘
// Dot products, norms, etc.
// Eigen // Matlab
x.norm() // norm(x). Note that norm(R) doesn't work in Eigen.
x.squaredNorm() // dot(x, x) Note the equivalence is not true for complex
x.dot(y) // dot(x, y)
x.cross(y) // cross(x, y) Requires #include <Eigen/Geometry>
矩阵单个元素操作
// Vectorized operations on each element independently
// Eigen // Matlab
R = P.cwiseProduct(Q); // R = P .* Q
R = P.array() * s.array();// R = P .* s
R = P.cwiseQuotient(Q); // R = P ./ Q
R = P.array() / Q.array();// R = P ./ Q
R = P.array() + s.array();// R = P + s
R = P.array() - s.array();// R = P - s
R.array() += s; // R = R + s
R.array() -= s; // R = R - s
R.array() < Q.array(); // R < Q
R.array() <= Q.array(); // R <= Q
R.cwiseInverse(); // 1 ./ P
R.array().inverse(); // 1 ./ P
R.array().sin() // sin(P)
R.array().cos() // cos(P)
R.array().pow(s) // P .^ s
R.array().square() // P .^ 2
R.array().cube() // P .^ 3
R.cwiseSqrt() // sqrt(P)
R.array().sqrt() // sqrt(P)
R.array().exp() // exp(P)
R.array().log() // log(P)
R.cwiseMax(P) // max(R, P)
R.array().max(P.array()) // max(R, P)
R.cwiseMin(P) // min(R, P)
R.array().min(P.array()) // min(R, P)
R.cwiseAbs() // abs(P)
R.array().abs() // abs(P)
R.cwiseAbs2() // abs(P.^2)
R.array().abs2() // abs(P.^2)
(R.array() < s).select(P,Q); // (R < s ? P : Q)
矩阵分块
// Matrix slicing and blocks. All expressions listed here are read/write.
// Templated size versions are faster. Note that Matlab is 1-based (a size N
// vector is x(1)...x(N)).
// Eigen // Matlab
x.head(n) // x(1:n)
x.head<n>() // x(1:n)
x.tail(n) // x(end - n + 1: end)
x.tail<n>() // x(end - n + 1: end)
//从元素x(i)到x(i+n-1)共n个元素，对Matrix<double,m,1>或Matrix<double,1,m>类型也适用
x.segment(i, n) // x(i+1 : i+n)
x.segment<n>(i) // x(i+1 : i+n)
//rows*cols个元素，从P(i,j)开始的一个矩阵块
P.block(i, j, rows, cols) // P(i+1 : i+rows, j+1 : j+cols)
P.block<rows, cols>(i, j) // P(i+1 : i+rows, j+1 : j+cols)
P.row(i) // P(i+1, :)
P.col(j) // P(:, j+1)
P.leftCols<cols>() // P(:, 1:cols)
P.leftCols(cols) // P(:, 1:cols)
P.middleCols<cols>(j) // P(:, j+1:j+cols)
P.middleCols(j, cols) // P(:, j+1:j+cols)
P.rightCols<cols>() // P(:, end-cols+1:end)
P.rightCols(cols) // P(:, end-cols+1:end)
P.topRows<rows>() // P(1:rows, :)
P.topRows(rows) // P(1:rows, :)
P.middleRows<rows>(i) // P(i+1:i+rows, :)
P.middleRows(i, rows) // P(i+1:i+rows, :)
P.bottomRows<rows>() // P(end-rows+1:end, :)
P.bottomRows(rows) // P(end-rows+1:end, :)
P.topLeftCorner(rows, cols) // P(1:rows, 1:cols)
P.topRightCorner(rows, cols) // P(1:rows, end-cols+1:end)
P.bottomLeftCorner(rows, cols) // P(end-rows+1:end, 1:cols)
P.bottomRightCorner(rows, cols) // P(end-rows+1:end, end-cols+1:end)
P.topLeftCorner<rows,cols>() // P(1:rows, 1:cols)
P.topRightCorner<rows,cols>() // P(1:rows, end-cols+1:end)
P.bottomLeftCorner<rows,cols>() // P(end-rows+1:end, 1:cols)
P.bottomRightCorner<rows,cols>() // P(end-rows+1:end, end-cols+1:end)
矩阵元素交换
// Of particular note is Eigen's swap function which is highly optimized.
// Eigen // Matlab
R.row(i) = P.col(j); // R(i, :) = P(:, i)
R.col(j1).swap(mat1.col(j2)); // R(:, [j1 j2]) = R(:, [j2, j1])
矩阵转置
// Views, transpose, etc; all read-write except for .adjoint().
// Eigen // Matlab
R.adjoint() // R*
R.transpose() // R'
R.diagonal() // diag(R)
x.asDiagonal() // diag(x)
R.transpose().colwise().reverse(); // rot90(R)
R.conjugate() // conj(R) 共轭
矩阵特征值计算
// Eigenvalue problems
// Eigen // Matlab
A.eigenvalues(); // eig(A);
A.eigenvectors(); // vec
EigenSolver<Matrix3d> eig(A); // [vec val] = eig(A)
eig.eigenvalues(); // diag(val)
eig.eigenvectors(); // vec
// For self-adjoint matrixes use SelfAdjointEigenSolver<>
//SelfAdjointEigenSolver是一种线性代数求解器，用于寻找实对称矩阵的特征值和特征向量
矩阵类型转换
Type conversion
// Eigen // Matlab
A.cast<double>(); // double(A)
A.cast<float>(); // single(A)
A.cast<int>(); // int32(A)
A.real(); // real(A)
A.imag(); // imag(A)
// if the original type equals destination type, no work is done
Eigen::Map
template<typename PlainObjectType, int MapOptions, typename StrideType>
class Eigen::Map< PlainObjectType, MapOptions, StrideType >
eigen中Map是一个模板类，用于将顺序容器中的元素（或者说是一段连续内存）表达成eigen中矩阵类型如Matrix或者Vector，而不会造成任何内存和时间上的开销。其操作的对象是顺序容器、数组等能获得指向一段连续内存的指针。
参数
-
PlainObjectType 映射后的eigen数据类型
-
MapOptions 指针所指对象的内存对齐方式，可选对齐方式如下，默认为
Eigen::Unaligned ( = 0)：enum Eigen::AlignmentType { Eigen::Unaligned, Eigen::Aligned8, Eigen::Aligned16, Eigen::Aligned32, Eigen::Aligned64, Eigen::Aligned128, Eigen::Aligned } -
StrideType 跨度类型，默认情况下map在数组的内存中是连续取得映射元素，可以通过该参数设置按照一定的跨度映射元素。可选的跨度有三种：Stride, InnerStride和OuterStride：
-
Stride 是下面两种跨度的基类，可同时指定内外跨度，有两种构造方式，如下：
template<int _OuterStrideAtCompileTime, int _InnerStrideAtCompileTime> class Stride { ... /** 默认构造，在编译时指定跨度值，跨度通过模板参数进行指定 */ Stride() ... /** 该构造用于在运行期指定跨度值 */ Stride(Index outerStride, Index innerStride) ... } -
InnerStride 数据相邻元素对应数组的指针的增量，其实就是Stride的外跨度为0的情况
-
OuterStride 相邻列/行（Eigen默认列优先，否则为行）的指针增量，其实就是Stride的内跨度为0的情况。列优先的意思为连续数据元素按一列一列的填充，Eigen中列优先的速度会快很多。
-
例子
MatrixXd mat(pts.size(), 3);
for(int i = 0; i < pts.size(); ++i){
mat.row(i) << pts[i];
}
Map<MatrixXd, 0, OuterStride<3>> mat(pts.data()->data(), pts,size(), 3);
线性代数
SVD分解
A = U Σ V H A=U\Sigma V^{H} A=UΣVH
JacobiSVD<typename _MatrixType, int QRPreconditioner>(const MatrixType& matrix, unsigned int computationOptions = 0)
{
compute(matrix, computationOptions);
}
BDCSVD<typename _MatrixType>(const MatrixType& matrix, unsigned int computationOptions = 0){
compute(matrix, computationOptions);
}
- matrix : the matrix to decompose
- computationOptions : 是否计算全部or非零奇异值对应的U,V酉矩阵，用
|号来同时指定多个选项- ComputeFullU
- ComputeThinU
- ComputeFullV
- ComputeThinV
Full和Thin不能同时指定
- QRPreconditioner : QR分解的方法
- ColPivHouseholderQRPreconditioner
- FullPivHouseholderQRPreconditioner
- HouseholderQRPreconditioner
- NoQRPreconditioner
求解线性方程组 Ax = b
上述SVD方法也可以用来求解线性方程组
MatrixXf m = MatrixXf::Random(4,4);
cout << "Here is the matrix m:" << endl << m << endl;
JacobiSVD<MatrixXf> svd(m,ComputeThinV | ComputeThinU);
cout << "Its singular values are:" << endl << svd.singularValues() << endl;
cout << "Its left singular vectors are the columns of the thin U matrix:" << endl << svd.matrixU() << endl;
cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;
VectorXf rhs=VectorXf::Zero(4);
rhs<<1,0,0,0;
cout << "Now consider this rhs vector:" << endl << rhs << endl;
cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(rhs) << endl;
// Solve Ax = b. Result stored in x. Matlab: x = A \ b.
x = A.ldlt().solve(b)); // A sym. p.s.d. #include <Eigen/Cholesky>
x = A.llt().solve(b)); // A sym. p.d. #include <Eigen/Cholesky>
x = A.lu().solve(b)); // Stable and fast. #include <Eigen/LU>
x = A.qr().solve(b)); // No pivoting. #include <Eigen/QR>
x = A.svd().solve(b)); // Stable, slowest. #include <Eigen/SVD>
// .ldlt() -> .matrixL() and .matrixD()
// .llt() -> .matrixL()
// .lu() -> .matrixL() and .matrixU()
// .qr() -> .matrixQ() and .matrixR()
// .svd() -> .matrixU(), .singularValues(), and .matrixV()
Eigen几何模块
#include<eigen3/Eigen/Geometry>//Eigen的几何模块
using namespace Eigen;//以下所有代码均在Eigen命名空间
头文件
旋转矩阵
旋转矩阵就是一个普通的3*3矩阵。
Matrix3d rotation_matrix;
//用旋转矩阵进行坐标变换
Vector3d v_rotated = rotation_matrix * v;
//旋转矩阵转四元数
Quaternionf quaternion(rotation_matrix);
quaternion=rotation_matrix;
//旋转矩阵转旋转向量
rotation_vector=rotation_matrix;
rotation_vector.fromRotationMatrix(rotation_matrix);
//旋转矩阵转欧拉角
euler_angles = rotation_matrix.eulerAngles(2,1,0);//ZYX顺序，R=Rz*Ry*Rx euler_angles[0]:yaw角 euler_angles[1]:pitch角 euler_angles[2]:roll角
euler_angles = rotation_matrix.eulerAngles(0,1,2);//XYZ顺序，R=Rx*Ry*Rz euler_angles[0]:roll角 euler_angles[1]:pitch euler_angles[2]:yaw角
旋转向量
//构造函数
AngleAxisd rotation_vector(M_PI/4,Vector3d(0,0,1));//同AngleAxis<double>，沿Z轴旋转45度
AngleAxisf rotation_vector(M_PI/4,Vector3d(0,0,1));//同AngleAxis<float>
//用AngleAxis进行坐标变换
Vector3d v_rotated = rotation_vector * v;
//旋转向量转旋转矩阵
rotation_vector.matrix();
rotation_vector.toRotationMatrix();
//旋转向量转欧拉角
Vector3d euler_angles=rotation_vector.matrix().eulerAngles(2,1,0);//ZYX顺序，R=Rz*Ry*Rx euler_angles[0]:yaw角 euler_angles[1]:pitch角 euler_angles[2]:roll角
Vector3d euler_angles=rotation_vector.matrix().eulerAngles(0,1,2);//XYZ顺序，R=Rx*Ry*Rz euler_angles[0]:roll角 euler_angles[1]:pitch euler_angles[2]:yaw角
//旋转向量转四元数
Quaterniond quaternion(rotation_vector);
quaternion=rotation_vector;
欧拉角
Vector3d euler_angles；
//欧拉角转旋转矩阵
Eigen::AngleAxisd roll(AngleAxisd(euler_angles(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitch(AngleAxisd(euler_angles(1),Vector3d::UnitY()));
Eigen::AngleAxisd yaw(AngleAxisd(euler_angles(0),Vector3d::UnitZ()));
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yaw*pitch*roll;//ZYX顺序，R=Rz*Ry*Rx
rotation_matrix=roll*pitch*yaw;//XYZ顺序，R=Rx*Ry*Rz
//欧拉角转旋转向量
AngleAxisd rotation_vector=roll*pitch*yaw;//XYZ顺序，R=Rx*Ry*Rz
//欧拉角转四元数
Quaterniond quaternion=yaw*pitch*roll;//ZYX顺序，R=Rz*Ry*Rx
四元数
//Quaterniond同Quaternion<double>
//Quaternionf同Quaternion<float>
Quaterniond quaternion(w,x,y,x);//四元数(w,x,y,z)构造函数
quaternion.coeffs();//将四元数转化为一个向量，顺序是(x,y,z,w)，w为实部，前三者为虚部
//使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = quaternion*v;//数学上是qvq^{-1}
quaternion.conjugate();//四元数的共轭
quaternion.inverse();//四元数的逆
//四元数转旋转向量
AngleAxisd rotation_vector(quaternion);
rotation_vector=quaternion;
//四元数转旋转矩阵
rotation_matri=quaternion.matrix();
rotation_matri=quaternion.toRotationMatrix();
//四元数转欧拉角
Vector3d euler_angles=quaternion.matrix().eulerAngles(2,1,0);//ZYX顺序，R=Rz*Ry*Rx
变换
/**
* \class Transform
*
* \brief Represents an homogeneous transformation in a N dimensional space
*
* \tparam Scalar the scalar type, i.e., the type of the coefficients
* \tparam Dim the dimension of the space
* \tparam Mode the type of the transformation. Can be:
* - #Isometry: Transformation is isometry.
* [R t]
* [0 1]
* - #Affine: the transformation is stored as a (Dim+1)^2 matrix,
* where the last row is assumed to be [0 ... 0 1].
* - #AffineCompact: the transformation is stored as a (Dim)x(Dim+1) matrix.
* - #Projective: the transformation is stored as a (Dim+1)^2 matrix
* without any assumption.
* \tparam _Options has the same meaning as in class Matrix. It allows to specify DontAlign and/or RowMajor.
* These Options are passed directly to the underlying matrix type.
**/
template<typename Scalar, int Dim, int Mode, int Options> class Transform
旋转
RotationType原生支持
- 任意标量(2D),
- 一个 D i m × D i m \rm Dim\times Dim Dim×Dim 矩阵
- Quaternion(3D),
- AngleAxis(3D)
//乘在原变换矩阵的右边
template<typename RotationType>
EIGEN_DEVICE_FUNC
inline Transform& rotate(const RotationType& rotation);
[ R t 0 1 ] ⏟ 原矩阵 [ R i n 0 0 1 ] ⏟ 输入旋转 = [ R R i n t 0 1 ] ⏟ 结果矩阵 \underbrace{ \begin{bmatrix} \boldsymbol{R}&\boldsymbol{t}\\ \bf{0}&1 \end{bmatrix}}_{原矩阵} \underbrace{ \begin{bmatrix} \bm{R}_{in} & \bf{0}\\ \bf{0}&1 \end{bmatrix}}_{输入旋转} =\underbrace{ \begin{bmatrix} \bm{R}\bm{R}_{in}&\bm{t}\\ \bf{0}&1 \end{bmatrix}}_{结果矩阵} 原矩阵 [R0t1]输入旋转 [Rin001]=结果矩阵 [RRin0t1]
//乘在原变换矩阵的左边
template<typename RotationType>
EIGEN_DEVICE_FUNC
inline Transform& prerotate(const RotationType& rotation);
[ R i n 0 0 1 ] ⏟ 输入旋转 [ R t 0 1 ] ⏟ 原矩阵 = [ R i n R R i n t 0 1 ] ⏟ 结果矩阵 \underbrace{ \begin{bmatrix} \bm{R}_{in}&\bf{0}\\ \bf{0}&1 \end{bmatrix}}_{输入旋转} \underbrace{ \begin{bmatrix} \boldsymbol{R}&\boldsymbol{t}\\ \bf{0}&1 \end{bmatrix}}_{原矩阵} =\underbrace{ \begin{bmatrix} \bm{R}_{in}\bm{R}&\bm{R}_{in}\bm{t}\\ \bf{0}&1 \end{bmatrix}}_{结果矩阵} 输入旋转 [Rin001]原矩阵 [R0t1]=结果矩阵 [RinR0Rint1]
平移
//输入参数是一个vector向量，vector向量代表的变换矩阵乘在原变换矩阵的右边
template<typename OtherDerived>
EIGEN_DEVICE_FUNC
inline Transform& translate(const MatrixBase<OtherDerived> &other);
[ R t 0 1 ] ⏟ 原矩阵 [ I t i n 0 1 ] ⏟ t i n 是输入位移向量 = [ R t + R t i n 0 1 ] ⏟ 结果矩阵 \underbrace{ \begin{bmatrix} \boldsymbol{R}&\boldsymbol{t}\\ \bf{0}&1 \end{bmatrix}}_{原矩阵} \underbrace{ \begin{bmatrix} \bf{I}&\boldsymbol{t}_{in}\\ \bf{0}&1 \end{bmatrix}}_{\bm{t}_{in}是输入位移向量} =\underbrace{ \begin{bmatrix} \bm{R}&\bf{t}+\boldsymbol{Rt}_{in}\\ \bf{0}&1 \end{bmatrix}}_{结果矩阵} 原矩阵 [R0t1]tin是输入位移向量 [I0tin1]=结果矩阵 [R0t+Rtin1]
//乘在变换矩阵的左边
template<typename OtherDerived>
EIGEN_DEVICE_FUNC
inline Transform& pretranslate(const MatrixBase<OtherDerived> &other);//
[ I t i n 0 1 ] ⏟ t i n 是输入位移向量 [ R t 0 1 ] ⏟ 原矩阵 = [ R t + t i n 0 1 ] ⏟ 结果矩阵 \underbrace{ \begin{bmatrix} \bf{I}&\boldsymbol{t}_{in}\\ \bf{0}&1 \end{bmatrix}}_{\bm{t}_{in}是输入位移向量} \underbrace{ \begin{bmatrix} \boldsymbol{R}&\boldsymbol{t}\\ \bf{0}&1 \end{bmatrix}}_{原矩阵} =\underbrace{ \begin{bmatrix} \bm{R}&\bf{t}+\boldsymbol{t}_{in}\\ \bf{0}&1 \end{bmatrix}}_{结果矩阵} tin是输入位移向量 [I0tin1]原矩阵 [R0t1]=结果矩阵 [R0t+tin1]
欧式变换
typedef Transform<double,3,Isometry> Isometry3d;
Isometry3d T = Isometry3d::Identity();//虽然称为3D，实质上是4*4矩阵
Isometry3d T(q);//用四元数旋转构造Transform矩阵
//设置欧式变换的旋转部分
T.rotate(rotation_vector);//按照AngleAxis进行旋转
T.rotate(q);//按照四元数进行旋转
T.rotate(rotation_matrix);//按照旋转矩阵进行旋转
//设置欧式变换的平移部分
T.pretranslate(Vector3d(1,2,3));//设置平移向量
T.rotation();//变换矩阵的旋转矩阵
T.translation();//变换矩阵的平移向量
T.matrix();//变换的
仿射变换
typedef Transform<double,3,Affine> Affine3d;
射影变换
typedef Transform<double,3,Projective> Projective3d;
Opencv与Eigen矩阵的相互转换
使用cv::cv2eigen 和 cv::eigen2cv进行相互转换
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core> // eigen 头文件必须放在 opencv 前面
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>//必须包含的头文件
int main()
{
cv::Mat c_data(2,2,CV_32F);
c_data = (cv::Mat_<float>(2,2) << 1,2,3,4);
/* cv::Mat ---> Eigen::Matrix */
Eigen::Matrix2d m_data;
cv::cv2eigen(c_data, m_data);
std::cout << m_data << std::endl;
/* Eigen::Matrix --> cv::Mat */
cv::Mat c_data2;
cv::eigen2cv(m_data, c_data2);
std::cout << c_data2 << std::endl;
return 0;
}
pcl与Eigen
坐标变换
#include<pcl/common/eigen.h>
tf::StampedTransform transform;
tf::Matrix3x3 m(transform.getRotation());
m.getRPY(rollCur, pitchCur, yawCur);
Eigen::Affine3f transNow；
pcl::getTransformation(xCur, yCur, zCur, rollCur, pitchCur, yawCur， transNow);
//pcl::getTransformation的逆运算
pcl::getTranslationAndEulerAngles(transNow,x,y,z,roll,pitch,yaw);
pcl::getTransformation和pcl::getTranslationAndEulerAngles都是3-2-1姿态序列欧拉角，两者互为逆运算。
R
z
R
y
R
x
=
[
c
2
c
3
−
s
3
s
2
c
3
c
2
s
3
c
3
s
2
s
3
−
s
2
0
c
2
]
[
1
0
0
0
c
1
−
s
1
∘
s
1
c
1
]
=
[
c
2
c
3
s
1
s
2
c
3
−
c
1
s
3
s
1
s
3
+
c
1
s
2
c
3
c
2
s
3
c
1
c
3
+
s
1
s
2
s
3
c
1
s
2
s
3
−
s
1
c
3
−
s
2
s
1
c
2
c
1
c
2
]
\begin{aligned} \boldsymbol{R}_{z}\boldsymbol{R}_{y}\boldsymbol{R}_{x} &=\begin{bmatrix} c_{2}c_{3} & -s_{3} & s_{2}c_{3} \\ c_{2}s_{3} & c_{3} & s_{2}s_{3} \\ -s_{2} & 0 & c_{2} \end{bmatrix}\begin{bmatrix} 1 & 0 & 0 \\ 0 & c_{1} & -s_{1} \\ \circ & s_{1} & c_{1} \end{bmatrix} \\ &=\begin{bmatrix} c_{2}c_{3} & s_{1}s_{2}c_{3}-c_{1}s_{3} & s_{1}s_{3}+c_{1}s_{2}c_{3} \\ c_{2}s_{3} & c_{1}c_{3}+s_{1}s_{2}s_{3} & c_{1}s_{2}s_{3}-s_{1}c_{3} \\ -s_{2} & s_{1}c_{2} & c_{1}c_{2} \end{bmatrix} \end{aligned}
RzRyRx=
c2c3c2s3−s2−s3c30s2c3s2s3c2
10∘0c1s10−s1c1
=
c2c3c2s3−s2s1s2c3−c1s3c1c3+s1s2s3s1c2s1s3+c1s2c3c1s2s3−s1c3c1c2
PCA计算点云的三个主方向
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn (new pcl::PointCloud<pcl::PointXYZ>);//输入点云
Eigen::Vector4f pcaCentroid;
pcl::compute3DCentroid(*cloudIn, pcaCentroid);
Eigen::Matrix3f covariance;
pcl::computeCovarianceMatrixNormalized(*cloudIn, pcaCentroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();
eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直
eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
// 另一种计算点云协方差矩阵特征值和特征向量的方式:通过pcl中的pca接口，如下，这种情况得到的特征向量类似
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn (new pcl::PointCloud<pcl::PointXYZ>);//输入点云
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(cloudIn);
Eigen::Vector3f eigen_values = pca.getEigenValues();//计算特征值
Eigen::MatrixXf eigen_vectors = pca.getEigenVectors();//计算特征向量
std::cerr << std::endl << "EigenVectors: " << eigen_vectors << std::endl;//计算特征向量
std::cerr << std::endl << "EigenValues: " << eigen_values << std::endl;//计算特征值
// Choose the principal component vector
Eigen::Vector3f project_vector = eigen_vectors.col(2)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojected (new pcl::PointCloud<pcl::PointXYZ>);
//pcl::PCA::project (InputCloud const &input, OutputCloud &projected, Eigen::Vector3f const &pcv);
// Project the input cloud onto the PCV
pcl::PCA::project (*input, *cloudPCAprojected, project_vector);
自制矩阵库
template<class T>
class Matrix
{
public:
int row,col;
T **pmat;
Matrix(int row,int col)
{
this->row=row;
this->col=col;
this->pmat=new T*[this->row];
for(int i=0;i<this->row;i++)
{
this->pmat[i]=new T[this->col];
}
for(int i=0;i<this->row;i++)
{
for(int j=0;j<col;j++)
{
(this->pmat)[i][j]=0;
}
}
}
Matrix(int row,int col,T m[])
{
this->row=row;
this->col=col;
this->pmat=new T*[this->row];
for(int i=0;i<this->row;i++)
{
this->pmat[i]=new T[this->col];
}
int count=0;
for(int i=0;i<this->row;i++)
{
for(int j=0;j<col;j++)
{
(this->pmat)[i][j]=m[count];
count++;
}
}
}
~Matrix()
{
for( int i=0; i<this->col; i++ )
{
delete [] this->pmat[i];
}
delete [] this->pmat;
}
void copy(const double m[])
{
int count=0;
for(int i=0;i<this->row;i++)
{
for(int j=0;j<this->col;j++)
{
(this->pmat)[i][j]=m[count];
count++;
}
}
}
void operator=(const Matrix& m)
{
for(int i=0;i<this->row;i++)
{
for(int j=0;j<this->col;j++)
{
(this->pmat)[i][j]=m.pmat[i][j];
}
}
}
Matrix operator+(const Matrix& m)
{
Matrix ans(this->row,this->col);
for(int i=0;i<this->row;i++)
{
for(int j=0;j<this->col;j++)
{
ans.pmat[i][j]=(this->pmat)[i][j]+m.pmat[i][j];
}
}
return ans;
}
Matrix operator-(const Matrix& m)
{
Matrix ans(this->row,this->col);
for(int i=0;i<this->row;i++)
{
for(int j=0;j<this->col;j++)
{
ans.pmat[i][j]=(this->pmat)[i][j]-m.pmat[i][j];
}
}
return ans;
}
Matrix operator*(const Matrix& mat2)
{
Matrix ans(this->row,mat2.col);
for(int i=0;i<this->row;i++)
{
for(int j=0;j<mat2.col;j++)
{
for(int k=0;k<this->col;k++)
{
ans.pmat[i][j]+=(this->pmat)[i][k]*mat2.pmat[k][j];
}
}
}
return ans;
}
Matrix transpose()
{
Matrix ans(this->row,this->col);
for(int i=0;i<this->row;i++)
{
for(int j=0;j<this->col;j++)
{
ans.pmat[i][j]=(this->pmat)[j][i];
}
}
return ans;
}
friend std::ostream & operator << (std::ostream &os, const Matrix &rightM)
{
os<<"调用重载插入运算符" << endl;
os<< "输出的矩阵为：" << endl;
for (int i = 0; i < rightM.row; i++)
{
for (int j = 0; j < rightM.col; j++)
{
if (j == 0)
os<< " | "; //为了美观，增加竖线分隔
os<< rightM.pmat[i][j] << " ";
}
os<< "|";
os<< endl;
}
return os;
} // 重载插入运算符实现矩阵直接输出
friend istream& operator>>(istream &is, Matrix&m){
for (int i = 0; i < m.row; i++){
for (int j = 0; j < m.col; j++){
is >> m.pmat[i][j];
}
}
return is;
}
};
464
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
