Eigen库使用（C++例程）

查看Eigen版本 

$ head -n 20 /usr/include/eigen3/Eigen/src/Core/util/Macros.h

#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 92

版本就是3.2.92

---

搞清旋转关系

 eigen_test.cc: 

#include <cmath>
#include <iostream>
#include <Eigen/Eigen>
 
// wrap the angle within [-PI, PI)
double WrapToPI(double ang_in_rad) {
  int c = ang_in_rad / (2.0 * M_PI);
  ang_in_rad -= c * (2.0 * M_PI);
  if (ang_in_rad < -M_PI) {
    ang_in_rad += 2.0 * M_PI;
  }
  if (ang_in_rad >= M_PI) {
    ang_in_rad -= 2.0 * M_PI;
  }
  return ang_in_rad;
}
 
int main(int argc, char *argv[]) {
  double roll = 30;
  double pitch = 45;
  double yaw = 90;
  std::cout << "\n指定欧拉角(roll pitch yaw): " << roll << " " << pitch << " " << yaw << std::endl;
 
  Eigen::Vector3d p1(0, 1, 0); // 点p在o1参考系下的坐标为(0, 1, 0)
  std::cout << "\n点p在o1参考系下的坐标p1: " << p1.transpose() << std::endl;
 
  Eigen::AngleAxisd v_21(yaw * M_PI / 180, Eigen::Vector3d::UnitZ()); // o1参考系绕其z轴(转yaw)顺时针旋转90度得到o2参考系
  Eigen::Matrix3d R_21 = v_21.matrix();
  Eigen::Vector3d p2 = R_21 * p1; // 点p在o2参考系下的坐标为(-1, 0, 0)
  std::cout << "\n点p在o2参考系下的坐标p2: " << p2.transpose() << std::endl;
 
  Eigen::AngleAxisd v_32(pitch * M_PI / 180, Eigen::Vector3d::UnitY()); // o2参考系绕其y轴(pitch)顺时针旋转45度得到o3参考系
  Eigen::Matrix3d R_32 = v_32.matrix();
  Eigen::Vector3d p3 = R_32 * p2; // 点p在o3参考系下的坐标为(-0.707, 0, 0.707)
  std::cout << "\n点p在o3参考系下的坐标p3: " << p3.transpose() << std::endl;
 
  Eigen::AngleAxisd v_43(roll * M_PI / 180, Eigen::Vector3d::UnitX()); // o3参考系绕其x轴(roll)顺时针旋转30度得到o4参考系
  Eigen::Matrix3d R_43 = v_43.matrix();
  Eigen::Vector3d p4 = R_43 * p3; // 点p在o4参考系下的坐标为(-0.707, -0.35, 0.61)
  std::cout << "\n点p在o4参考系下的坐标p4: " << p4.transpose() << std::endl;
 
  Eigen::Matrix3d R_41 = R_43 * R_32 * R_21; // 先绕z轴顺时针旋转90度，再绕y轴顺时针旋转45度，最后绕x轴顺时针旋转30度
  // Eigen::Matrix3d R_41 = v_43 * v_32 * v_21;
  p4 = R_41 * p1; // 点p在o4参考系下的坐标为(-0.707, -0.353, 0.612)
  std::cout << "\n点p在o4参考系下的坐标p4: " << p4.transpose() << std::endl;
 
  // 旋转矩阵->欧拉角
  Eigen::Vector3d euler_angles = R_41.eulerAngles(0, 1, 2); // (0,1,2) 表示分别绕XYZ轴顺序（与上面旋转顺序相反），即按roll,pitch,yaw顺序，顺时针为正
  // Euler's angles are not unique.
  // eigen has two sets of euler angles: (a, b, c) or (pi+a, pi-b, pi+c)
  // In your XYZ convention, both (0, pi, pi) and (pi, 0, 0) represents the same rotation, and both are correct.
  // The Eigen::eulerAngles method consistently chooses to minimize first angles.
  if (std::fabs(euler_angles(1)) > M_PI / 2) {
    euler_angles(0) = WrapToPI(M_PI + euler_angles(0));
    euler_angles(1) = WrapToPI(M_PI - euler_angles(1));
    euler_angles(2) = WrapToPI(M_PI + euler_angles(2));
  }
  std::cout << "\n旋转矩阵->欧拉角(roll pitch yaw): " << euler_angles.transpose() * 180 / M_PI << std::endl; // 30 45 90
 
  return 0;
}

输出：

---

不同旋转表示及相互转换

eigen_test.cc:  

#include <cmath>
#include <iostream>
#include <Eigen/Eigen>
 
int main(int argc, char *argv[]) {
  // 单位四元素
  Eigen::Quaterniond q = Eigen::Quaterniond(1, 0, 0, 0); // (w,x,y,z)
  // Eigen::Quaterniond q(1, 0, 0, 0); // (w,x,y,z)
  // Eigen::Quaterniond q(Eigen::Vector4d(0, 0, 0, 1)); // (x,y,z,w)
  std::cout << "\n单位四元素:\n" << q.coeffs() << std::endl; // (x,y,z,w)
 
  // 单位旋转矩阵
  Eigen::Matrix3d rotation_matrix3d = Eigen::Matrix3d::Identity();
  std::cout << "\n单位旋转矩阵:\n" << rotation_matrix3d << std::endl;
 
  // 旋转向量（轴角）
  Eigen::AngleAxisd angle_axis(M_PI / 4, Eigen::Vector3d(0, 0, 1)); // 绕z轴顺时针旋转45°(yaw)
  std::cout << "\n旋转向量:\naxi: " << angle_axis.axis().transpose() << ", angle: " << angle_axis.angle() * 180 / M_PI << std::endl;
 
  // 欧拉角
  Eigen::Vector3d euler_angles(0, 0, 45); // roll pitch yaw（自定义）
  std::cout << "\n欧拉角:\n(roll pitch yaw) = " << euler_angles.transpose() << std::endl;
 
  // 旋转向量->旋转矩阵
  rotation_matrix3d = angle_axis.matrix();
  // rotation_matrix3d = angle_axis.toRotationMatrix();
  std::cout << "\n旋转向量->旋转矩阵:\n" << rotation_matrix3d << std::endl;
 
  // 旋转矩阵->旋转向量（轴角）
  angle_axis.fromRotationMatrix(rotation_matrix3d);
  // angle_axis = rotation_matrix3d;
  std::cout << "\n旋转矩阵->旋转向量(轴角):\naxi: " << angle_axis.axis().transpose() << ", angle: " << angle_axis.angle() * 180 / M_PI << std::endl;
 
  // 旋转向量（轴角）->四元素
  q = Eigen::Quaterniond(angle_axis);
  std::cout << "\n旋转向量(轴角)->四元素:\n(w x y z) = " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl;
 
  // 四元素->旋转向量（轴角）
  angle_axis = q;
  std::cout << "\n四元素->旋转向量(轴角):\naxi: " << angle_axis.axis().transpose() << ", angle: " << angle_axis.angle() * 180 / M_PI << std::endl;
 
  // 旋转矩阵->四元素
  q = Eigen::Quaterniond(rotation_matrix3d);
  // q = rotation_matrix3d;
  std::cout << "\n旋转矩阵->四元素:\n(w x y z) = " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl;
 
  // 四元素->旋转矩阵
  rotation_matrix3d = q.matrix();
  // rotation_matrix3d = q.toRotationMatrix();
  std::cout << "\n四元素->旋转矩阵:\n" << rotation_matrix3d << std::endl;
 
  // 旋转矩阵->欧拉角
  euler_angles = rotation_matrix3d.eulerAngles(0, 1, 2);
  std::cout << "\n旋转矩阵->欧拉角:\n(roll pitch yaw) = " << euler_angles.transpose() * 180 / M_PI << std::endl;
 
  return 0;
}

输出： 

---

基础用法

eigen_test.cc:  

#include <cmath>
#include <iostream>
#include <Eigen/Eigen>
 
int main(int argc, char *argv[]) {
  // 向量(列向量)
  Eigen::Vector3d v1(0, 0, 0); // 声明并定义
  v1.y() = 1;
  v1[2] = 2;
  std::cout << "v1: " << v1.transpose() << std::endl;
 
  Eigen::Vector3d v2;
  v2 << 2, 2, 2; // 先声明后定义
  std::cout << "v2: " << v2.transpose() << std::endl;
 
  Eigen::Vector3d t;
  t.setZero(); // 各分量设为0
  // t = Eigen::Vector3d::Zero();
  std::cout << "t: " << t.transpose() << std::endl;
  t.setOnes(); // 各分量设为1
  // t = Eigen::Vector3d::Ones();
  std::cout << "t: " << t.transpose() << std::endl;
 
  // 矩阵
  Eigen::Matrix<double,3,4> M;
  M << 1,0,0,1,
       0,2,0,1,
       0,0,1,1;
  M(1,1) = 1;
  std::cout << "M:\n" << M << std::endl;
 
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  std::cout << "R:\n" << R << std::endl;
 
  // 变换矩阵(4x4)
  Eigen::Matrix4d T;
  T << R, t, 0, 0, 0, 1;
  std::cout << "T:\n" << T << std::endl;
 
  // 数学运算
  v2 = R.inverse()*v2 - t;
  std::cout << "v2: " << v2.transpose() << std::endl;
  std::cout << "v2模长: " << v2.norm() << std::endl;
  std::cout << "v2单位向量: " << v2.normalized().transpose() << std::endl;
  std::cout << "v1点乘v2: " << v1.dot(v2) << std::endl;
  std::cout << "v1叉乘v2: " << v1.cross(v2).transpose() << std::endl; // 叉乘只能用于长度为3的向量
 
  // 块操作
  R = T.block<3, 3>(0, 0);
  t = T.block<3, 1>(0, 3);
  std::cout << "旋转R:\n" << T.topLeftCorner(3, 3) << std::endl;
  std::cout << "平移t: " << T.topRightCorner(3, 1).transpose() << std::endl;
 
  // 欧式变换矩阵(Isometry)
  Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity(); // 虽然称为3d，实质上是4x4的矩阵(旋转R+平移t)
 
  // 旋转部分赋值
  // T1.linear() = Eigen::Matrix3d::Identity();
  // T1.linear() << 1, 0, 0, 0, 1, 0, 0, 0, 1;
  // T1.rotate(Eigen::Matrix3d::Identity());
  T1.rotate(Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d(0,0,1)));
 
  // 平移部分赋值
  // T1.pretranslate(Eigen::Vector3d(1, 1, 1));
  T1.translation() = Eigen::Vector3d(1, 1, 1);
 
  std::cout << "T1:\n" << T1.matrix() << std::endl; // 输出4x4变换矩阵
  std::cout << "R1:\n" << T1.linear().matrix() << std::endl; // 输出旋转部分
  std::cout << "t1:\n" << T1.translation().transpose() << std::endl; // 输出平移部分
    
  Eigen::Quaterniond q(T1.linear());
  std::cout << "q: " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl;
 
  Eigen::Isometry3d T2(q);
  T2(0,3) = 1;
  T2(1,3) = 2;
  T2(2,3) = 3;
  std::cout << "T2:\n" << T2.matrix() << std::endl;
 
  Eigen::Vector3d v3(1,1,0);
  v3 = T1 * v3; // 相当于R1*v1+t1，隐含齐次坐标(1，1，0，1)
  std::cout << "v3: " << v3.transpose() << std::endl;
 
  // 仿射变换矩阵(Affine3d)
  Eigen::Translation3d t;
  Eigen::Quaterniond q;
  Eigen::Affine3d T = t * q;
 
  return 0;
}

输出：

Eigen::MatrixXd B = Eigen::MatrixXd::Identity(6, 5);
Eigen::VectorXd b(5);
b << 1, 4, 6, -2, 0.4;
Eigen::VectorXd Bb = B * b;
std::cout << "The multiplication of B * b is " << std::endl << Bb << std::endl;
 
Eigen::MatrixXd A(3, 2);
A << 1, 2,
2, 3,
3, 4;
Eigen::MatrixXd B = A.transpose();// the transpose of A is a 2x3 matrix
Eigen::MatrixXd C = (B * A).inverse();// computer the inverse of BA, which is a 2x2 matrix
 
Eigen::MatrixXd A = Eigen::MatrixXd::Random(7, 9);
Eigen::MatrixXd A = Eigen::MatrixXd::Random(7, 9);
std::cout << "The element at fourth row and 7the column is " << A(3, 6) << std::endl;
Eigen::MatrixXd B = A.block(1, 2, 3, 3);
std::cout << "Take sub-matrix whose upper left corner is A(1, 2)" << std::endl << B << std::endl;
Eigen::VectorXd a = A.col(1); // take the second column of A
Eigen::VectorXd b = B.row(0); // take the first row of B
Eigen::VectorXd c = a.head(3);// take the first three elements of a
Eigen::VectorXd d = b.tail(2);// take the last two elements of b
 
Eigen::Quaterniond q1(2, 0, 1, -3); 
q1.normalize();
std::cout << "To represent rotation, we need to normalize it such that its length is " << q1.norm() << std::endl;
 
Eigen::Vector3d v(1, 2, -1);
Eigen::Quaterniond q2;
q2.w() = 0;
q2.vec() = v;
Eigen::Quaterniond q = q1 * q2 * q1.inverse(); 
 
Eigen::Quaterniond a = Eigen::Quterniond::Identity();

Eigen旋转内插值

eigen_test.cc:

#include <cmath>
#include <iostream>
#include <Eigen/Eigen>
 
int main() {
  Eigen::AngleAxisd angle_axis1(M_PI / 6, Eigen::Vector3d(0, 0, 1)); // 沿z轴(yaw)顺时针旋转30°
  Eigen::Quaterniond q1 = Eigen::Quaterniond(angle_axis1);
  Eigen::Vector3d t1(3, 3, 3);
  Eigen::AngleAxisd angle_axis2(M_PI / 2, Eigen::Vector3d(0, 0, 1)); // 沿z轴(yaw)顺时针旋转90°
  Eigen::Quaterniond q2 = Eigen::Quaterniond(angle_axis2);
  Eigen::Vector3d t2(9, 9, 9);
  double ratio = 1.0 / 3;
 
  auto q = q1.slerp(ratio, q2);
  q.normalize();
  const auto &t = (1 - ratio) * t1 + ratio * t2;
  Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
  // Eigen::Matrix4d T{Eigen::Matrix4d::Identity()};
  T.block<3, 3>(0, 0) = q.toRotationMatrix();
  T.block<3, 1>(0, 3) = t;
 
  Eigen::Vector3d euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
  std::cout << "yaw pitch roll: " << euler_angles.transpose() * 180 / M_PI << std::endl;
  std::cout << "t: " << t.transpose() << std::endl;
 
  return 0;
}

输出：

---

解线性方程组

Eigen提供了解线性方程的计算方法，包括LU分解法，QR分解法，SVD（奇异值分解）、特征值分解等。对于一般形如的线性系统，解方程的方式一般是将矩阵A进行分解，当然最基本的方法是高斯消元法。

Eigen内置的解线性方程组的算法如下表所示： 

  eigen_test.cc:  

#include <iostream>
#include <Eigen/Dense>
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
 
using namespace std;
using namespace Eigen;
 
int main() {
    // Basic linear solving
    Matrix3f A;
    Vector3f b;
    A << 1,2,3,  4,5,6,  7,8,10;
    b << 3, 3, 4;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "Here is the vector b:\n" << b << endl;
    Vector3f x = A.colPivHouseholderQr().solve(b);
    cout << "The solution is:\n" << x << endl;
 
    Matrix2f A, b;
    LLT<Matrix2f> llt;
    A << 2, -1, -1, 3;
    b << 1, 2, 3, 1;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "Here is the right hand side b:\n" << b << endl;
    cout << "Computing LLT decomposition..." << endl;
    llt.compute(A);
    cout << "The solution is:\n" << llt.solve(b) << endl;
    A(1,1)++;
    cout << "The matrix A is now:\n" << A << endl;
    cout << "Computing LLT decomposition..." << endl;
    llt.compute(A);
    cout << "The solution is now:\n" << llt.solve(b) << endl;
 
    Matrix2f A, b;
    A << 2, -1, -1, 3;
    b << 1, 2, 3, 1;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "Here is the right hand side b:\n" << b << endl;
    Matrix2f x = A.ldlt().solve(b);
    cout << "The solution is:\n" << x << endl;
 
    // 计算矩阵的特征值和特征向量
    Matrix2f A;
    A << 1, 2, 2, 3;
    cout << "Here is the matrix A:\n" << A << endl;
    SelfAdjointEigenSolver<Matrix2f> eigensolver(A);
    if (eigensolver.info() != Success) abort();
    cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
    cout << "Here's a matrix whose columns are eigenvectors of A \n"
         << "corresponding to these eigenvalues:\n"
         << eigensolver.eigenvectors() << endl;
 
    // 计算矩阵的逆和行列式
    Matrix3f A;
    A << 1, 2, 1,
         2, 1, 0,
         -1, 1, 2;
    cout << "Here is the matrix A:\n" << A << endl;
    cout << "The determinant of A is " << A.determinant() << endl;
    cout << "The inverse of A is:\n" << A.inverse() << endl;
 
    // BDCSVD解最小二乘（推荐）
    MatrixXf A = MatrixXf::Random(3, 2);
    cout << "Here is the matrix A:\n" << A << endl;
    VectorXf b = VectorXf::Random(3);
    cout << "Here is the right hand side b:\n" << b << endl;
    cout << "The least-squares solution is:\n"
         << A.bdcSvd(ComputeThinU | ComputeThinV).solve(b) << endl;
 
    // JacobiSVD解最小二乘
    Eigen::Matrix3f H;
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(H, Eigen::ComputeFullU | 
    Eigen::ComputeFullV);
    Eigen::Matrix3f U = svd.matrixU();
    Eigen::Matrix3f V = svd.matrixV();
 
    // AX = 0
    // (AX)`(AX)
    // X`(A`A)X
    // 求特征值和特征向量
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> self_adjoint_solver;
    self_adjoint_solver.compute(ATA);
    Eigen::Matrix3d eigen_values = self_adjoint_solver.eigenvalues().asDiagonal(); // The eigenvalues are sorted in increasing order.
    Eigen::Matrix3d eigen_vectors = self_adjoint_solver.eigenvectors();
    Eigen::Vector3d eigen_vector = eigen_vectors.col(0);
    eigen_values(0, 0) = 0;
    ATA = eigen_vectors * eigen_values * eigen_vectors.inverse();
 
    Eigen::EigenSolver<Eigen::Matrix4d> general_solver;
    general_solver.compute(ATA);
    cout << "eigenvalues:\n" << general_solver.eigenvalues();
    cout << "eigenvectors:\n" << general_solver.eigenvectors();
    //wxyz = general_solver.eigenvectors().col(0);
 
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(test)
 
set(CMAKE_CXX_STANDARD 11)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 
find_package(Eigen3)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
 
add_executable(eigen_test eigen_test.cc)
target_link_libraries(eigen_test ${Eigen_LIBS})

强制类型转换

Eigen::Matrix4f v1;
const Eigen::Matrix4d v2 = v1.cast<double>();
 
Eigen::Matrix4d v1;
Eigen::Matrix4f v2 = v1.template cast<float>();

 Eigen::Matrix和cv::Mat相互转换

Eigen::Matrix3d eigen_R;
cv::Mat cv_R;
cv::cv2eigen(cv_R, eigen_R);
cv::eigen2cv(eigen_R, cv_R);

Eigen的SSE兼容，内存分配，和std容器的兼容理解

SSE支持128bit的多指令并行，但是有个要求是处理的对象必须要在内存地址以16byte整数倍的地方开始。不过这些细节Eigen在做并行化的时候会自己处理。但是，如果把一些Eigen的结构放到std的容器里面，比如vector，map。这些容器会把一个一个的Eigen结构在内存里面连续排放。

Eigen提供了两种方法来解决：

1、使用特别的内存分配对象。

std::map<int, Eigen::Vector4f, std::less<int>, Eigen::aligned_allocator<std::pair<const int, Eigen::Vector4f> > >

std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d>>

2、在对象定义的时候，使用特殊的宏，注意必须在所有Eigen对象出现前使用这个宏。

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION

有这个问题的Eigen结构包括：

Eigen::Vector2d
Eigen::Vector4d
Eigen::Vector4f
Eigen::Matrix2d
Eigen::Matrix2f
Eigen::Matrix4d
Eigen::Matrix4f
Eigen::Affine3d
Eigen::Affine3f
Eigen::Quaterniond
Eigen::Quaternionf

另外如果上面提到的这些结构作为一个对象的成员，这个时候需要在类定义里面使用另外一个宏

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Eigen库中的Map类

Map类用于通过C++中普通的连续指针或者数组 （raw C/C++ arrays）来构造Eigen里的Matrix类，这就好比Eigen里的Matrix类的数据和raw C++array 共享了一片地址，也就是引用。

1. 比如有个API只接受普通的C++数组，但又要对普通数组进行线性代数操作，那么用它构造为Map类，直接操作Map就等于操作了原始普通数组，省时省力。

2. 再比如有个庞大的Matrix类，在一个大循环中要不断读取Matrix中的一段连续数据，如果你每次都用block operation 去引用数据，太累（虽然block operation 也是引用类型）。于是就事先将这些数据构造成若干Map，那么以后循环中就直接操作Map就行了。

实际上Map类并没有自己申请一片空内存，只是一个引用，所以需要构造时初始化，或者使用Map的指针。

引申一下，Eigen里 ref 类也是引用类型，Armadillo 里 subview 都是引用类型，

Eigen开发人说的

The use 'sub' as a Matrix or Map. Actually Map, Ref, and Block inherit from the same base class. You can also use Block.

所以说了这么多，就一句话 Map 就是个引用。