Eigen notes

Quaternion

Constructor

The commonly used quaternion formats are Quaternionf(float)and , and determine the data type Quaterniond(double)in the template class . The official introduction has seven construction methods, but the commonly used ones are generally as follows:Scalar

• direct assignment

Eigen::Quaternion< _Scalar, _Options >::Quaternion( const Scalar & w,const Scalar & x,const Scalar &y,const Scalar & z ) 	
//for example
Quaterniond q(1.0, 0.0, 0.0, 0.0);

Pay attention to the order of quaternion assignment in Eigen, the real number w is first; but in fact its internal storage order is [xyzw]. In fact, when the coefficients are output later, they are also output in the order of internal storage.

• Construct from rotation matrix or vector

Eigen::Quaternion< _Scalar, _Options >::Quaternion(const MatrixBase< Derived > & other)	
//for example
Matrix3d mat;
Quaterniond q(mat);
VectorXd vq(4);
vq<<1.0, 0, 0, 0;
Quaterniond qv(vq);

• Constructed from an array
  The order of the array should be [wxyz]

Eigen::Quaternion< _Scalar, _Options >::Quaternion	(const Scalar * data)	

Common functions

• output coefficient

q.coeffs();     //[x y z w]

• output imaginary part

q.vec();    //[x y z]

The above two outputs are output in the form of vector Vector in Eigen

• Output rotation matrix
  It should be noted that only the unit quaternion represents the rotation matrix, so the quaternion must be unitized first

q.normalized();	//important
Matrix3d R=q.toRotationMatrix();

• Conjugate/that is, reverse rotation
  is generally not used inverse. When representing rotation (the norm is 1), conjugate can represent the opposite rotation.

//q.inverse();
q.conjugate();

• traverse elements

cout<<q.w()<<"  "<<q.x()<<"  "<<q.y()<<"  "<<q.z()<<endl;

Linear Interpolation Slerp

Spherical linear interpolation Slerpis a technique that performs interpolation in quaternion space for smooth rotational transitions between two quaternions. In Eigen, the quaternion class (Quaternion) provides the slerp method for spherical linear interpolation between two quaternions.

Here are slerpthe parameters and return values:

template<typename OtherDerived>
Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;

parameter:

• t : Interpolation coefficient, the range is [0, 1], which means to return the position of the quaternion between two quaternions, t=0 means to return the current quaternion, and t=1 means to return the target quaternion.
• other : Another quaternion to use as the target for interpolation.
• Return value: Returns the quaternion, the result of performing spherical linear interpolation between the current quaternion and the target quaternion.

Code example:

Eigen::Quaterniond q1(1, 0, 0, 0); // 初始四元数
Eigen::Quaterniond q2(0, 1, 0, 0); // 目标四元数
double t = 0.5; // 插值系数，范围为[0, 1]

Eigen::Quaterniond q = q1.slerp(t, q2); // 执行球面线性插值

// 输出结果
std::cout << "Interpolated Quaternion: " << q.coeffs().transpose() << std::endl;

transformation matrix

1. Eigen::Isometry3d constructs a transformation matrix

1.1. Assign values ​​to each element

Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
T1(0,0) = 1.000000e+00, T1(0,1) = 1.197624e-11, T1(0,2) = 1.704639e-10, T1(0,3) = 3.214096e-14;
T1(1,0) = 1.197625e-11, T1(1,1) = 1.197625e-11, T1(1,2) = 3.562503e-10, T1(1,3) = -1.998401e-15;
T1(2,0) = 1.704639e-10, T1(2,1) = 3.562503e-10, T1(2,2) = 1.000000e+00, T1(2,3) = -4.041212e-14;
T1(3,0) =            0, T1(3,1) =            0, T1(3,2) =            0, T1(3,3) =             1;

1.2. By rotation matrix and translation vector

Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity();
rotation_matrix1 << 1.000000e+00, 1.197624e-11, 1.704639e-10,
                    1.197625e-11, 1.000000e+00, 3.562503e-10,
                    1.704639e-10, 3.562503e-10, 1.000000e+00;
Eigen::Vector3d t1;
t1 <<  3.214096e-14, -1.998401e-15, -4.041212e-14;
    
T1=Eigen::Isometry3d::Identity();
T1.rotate ( rotation_matrix1 );
T1.pretranslate ( t1 );

Note that the matrix assignment cannot be directly transformed, and an error will be reported like this

T1<< 1.000000e+00, 1.197624e-11, 1.704639e-10, 3.214096e-14,
     1.197625e-11, 1.197625e-11, 3.562503e-10, -1.998401e-15,
     1.704639e-10, 3.562503e-10, 1.000000e+00, -4.041212e-14,
                0,            0,            0,              1;

2. Eigen::Matrix4d constructs transformation matrix

The method of assigning a value to each element is feasible, and what I use here is to assign values ​​​​by matrix blocks
Matrix.block<Rows, Cols>(startRow, startCol);

// ----3.eigen::matrix4d----
Eigen::Matrix4d T2;
T2.setIdentity();
T2.block<3,3>(0,0) = rotation_matrix1;
T2.topRightCorner(3, 1) = t1;
//T2.topRightCorner<3, 1>() = t1;

3. Convert the transformation matrix into a quaternion + translation vector

Use Eigen::Quaterniond to construct quaternions

 Eigen::Quaterniond q(mat.block<3,3>(0,0));

get the translation vector

Eigen::Vector3d translation(mat.block<3,1>(0,3));

Create matrix type vector container

std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d>>point;   
Eigen::Vector3d v(x,y,z);
points.push_back(v);

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{
    
    
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}

extract element operation

For a Eigen::Matrix3ftype of data x, when you want to extract the elements of the third row and the first column:

• Use x.row(2).col(0)the operation and assign it to a variable, prompting that the Eigen... type value cannot be assigned to the past;
• Use x(2,0)operations to extract elements and assign values.

In Eigen, you can use the column block of the matrix to get the rightmost column. For a 4*4 matrix, you can use matrix.col(3)to get the rightmost column. If you want to convert it to Vector3da type, you can use matrix.col(3).head<3>()to get the first three elements.

Matrix4d matrix;
// ... Initialize matrix
Vector3d right_column = matrix.col(3).head<3>();

Reductions

maxCoeff(),minCoeff()

When using maxCoeff()and minCoeff()functions, we can seek the largest element and the smallest element, but if we want to return its position, we need to give relevant parameters, and we need to use the type of the parameter type, for Indexexample:

colwise(), rowwise()

ReductionsIt can also be used partially, that is, what is returned is not a value, but a set of values, which can be regarded as a dimensionality reduction operation, and the function used is colwise(), rowwise(). For example:

Mat.colwise()It is understood as looking at each column of the matrix separately, and then applying maxCoeff()the function, that is, finding the maximum value of each column. It should be noted that colwisewhat is returned is a row vector (column direction dimensionality reduction), and rowwisewhat is returned is a column vector (row direction dimensionality reduction).

The difference between norm, normalize and normalized in Eigen

norm()

For Vector, normthe return is the two-norm of the vector, that is, for

example:

Vector2d vec(3.0,4.0);
cout << vec.norm() << endl;	//输出5

For Matrix, normreturns the Frobenius Norm of the matrix, ie

for example:

Matrix2d mat;
mat << 1,2
    3,4;
cout << mat.norm() << endl;    //输出sqrt(1*1+2*2+3*3+4*4),即sqrt(30) = 5.47723

normalize()

After a clear norm()definition, normalize()it is actually dividing each element of itself by its norm. The return value is void.

For example:

vec.normalize();
cout << vec << endl;    //输出：      0.6
                       //            0.8
 
mat.normalize();        //mat各元素除以mat.norm()
cout << mat << endl;    

normalized()

And similar normalized()to normalize(), normalize()it just modifies itself, and normalized()returns a new one Vector/Matrixwithout changing the original matrix.

Conversion between quaternions, Euler angles, rotation matrices, and rotation vectors

rotation vector

1.0 Initialize the rotation vector: the rotation angle is alpha, and the rotation axis is (x, y, z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))

1.1 Rotation vector to rotation matrix

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

1.2 Rotation vector to Euler angle (XYZ, namely RPY)

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

1.3 Convert rotation vector to quaternion

Eigen::Quaterniond quaternion(rotation_vector);
//or
Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

Quaternion

Quaternion to Euler angle (XYZ, ie RPY)

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);

Euler angle

Rotation Matrix to Euler Angles

Eigen::Vector3d euler_angles;
euler_angles = rotation_matrix.eulerAngles(2, 1, 0);  // 按照 z-y-x 的顺序进行旋转
 // 输出欧拉角
std::cout << "roll: " << euler_angles(0) << ", pitch: " << euler_angles(1) << ", yaw: " << euler_angles(2);

Eigen's positional bonus specified in the matrix

In Eigen, you can use operator() methods to access elements in a matrix, and you can use assignment operators to directly add a specified value to that location. Here is a simple example:

Eigen::MatrixXd mat(2, 2);
  mat << 1, 2,
         3, 4;
  // 在第一行第二列的位置上加上3
  mat(0, 1) += 3;

memory alignment problem

In the process of using the Eigen library, problems like this often appear:

/usr/include/eigen3/Eigen/src/Core/DenseStorage.h:128: Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 32>::plain_array() [with T = float; int Size = 8; int MatrixOrArrayOptions = 0]: Assertion `(reinterpret_cast<size_t>(eigen_unaligned_array_assert_workaround_gcc47(array)) & (31)) == 0 && “this assertion is explained here: " “http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html” " **** READ THIS WEB PAGE !!! ****”’ failed.

The reason is that because the Eigen library uses SSE acceleration, it needs to be aligned according to 128 bits, which leads to a Fixed-size vectorizable Eigen objects16-byte alignment. Generally, Eigen has already done the alignment. For example, the Eigen library overloads the new operation. However, in some cases, this write alignment The setting was overridden, causing the above assertion error.

• If you don't need Eigen's acceleration, use the following three methods to solve this kind of error:
  Use unaligned objects: Objects in Eigen provide various constructors, and use unaligned constructors to construct objects. Use Eigen_DONT_VECTORIZEmacros: this disables all 16-byte statically aligned code while using Eigen_DONT_VECTORIZEand EIGEN_DISABLE_UNALIGNED_ASSERTmacros: this preserves 16-byte pair codes, but disables vectorization

• Otherwise, according to the following conditions:

• There are classes in the program that contain Eigen objects, for example:

classFoo{
    
    

 	Eigen::Vector2d v; //Fixed-size vectorizable Eigen objects
 };
 
 Foo *foo = 
 new Foo;

changed to

classFoo{
    
    
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW //此处添加宏定义
 	Eigen::Vector2d v; //Fixed-size vectorizable Eigen objects
 };
 
 Foo *foo = 
 new Foo;

• STL containers or manual memory allocation

#include <Eigen/StdVector>

std::map<int, Eigen::Matrix4d, std::less<int>, Eigen::aligned_allocator<std::pair<const int, Eigen::Matrix4d>>>poses;
std::vector<Eigen::vector4f,Eigen::aligned_allocator<Eigen::vector4f> >

g2o error: Invalid sizes when resizing a matrix or array."' failed A possible reason

When using g2o to solve nonlinear optimization problems, a runtime error message
/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:281: void Eigen::PlainObjectBase::resize(Eigen::Index, Eigen ::Index) [with Derived = Eigen::Matrix<double, 3, 3>; Eigen::Index = long int]: Assertion `(!(RowsAtCompileTime!=Dynamic) || ​​(rowsRowsAtCompileTime)) && (!(ColsAtCompileTime!=Dynamic) || (colsColsAtCompileTime)) && (!(RowsAtCompileTimeDynamic && MaxRowsAtCompileTime!=Dynamic) || (rows<=MaxRowsAtCompileTime)) && (!(ColsAtCompileTimeDynamic && MaxColsAtCompileTime!=Dynamic) || ​​(cols<=MaxColsAtCompileTime)) && rows>=0 && cols>=0 && “Invalid sizes when resizing a matrix or array.”' failed. That is, the dimension of g2o::BlockSolverTraits may be
written wrong.

template <int _PoseDim, int _LandmarkDim>
  struct BlockSolverTraits

The above is the definition of BlockSolverTraits in g2o. PoseDim can be roughly understood as the dimension of the optimized node, and LandmarkDim can be roughly understood as the dimension of the error term.

Or you can use the following class instead, so that you don't have to worry about the dimension problem, it will calculate it at runtime.

template <>
  struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>