文章目录
四元数
构造函数
常用的四元数格式有Quaternionf(float)和Quaterniond(double)，模板类中的Scalar决定数据类型。
官方介绍有七种构造方式，不过常用的一般有以下几种：
- 直接赋值
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);
要注意Eigen中四元数赋值的顺序，实数w在首；但是实际上它的内部存储顺序是[x y z w]。实际上后面输出系数的时候也是按内部存储顺序输出
- 从旋转矩阵或向量构造
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);
- 从数组构造
数组的顺序应该是[w x y z]
Eigen::Quaternion< _Scalar, _Options >::Quaternion (const Scalar * data)
常用函数
- 输出系数
q.coeffs(); //[x y z w]
- 输出虚部
q.vec(); //[x y z]
以上两种输出都是以Eigen中的向量Vector形式输出
- 输出旋转矩阵
要注意的是，只有单位四元数才表示旋转矩阵，所以要先对四元数做单位化
q.normalized(); //important
Matrix3d R=q.toRotationMatrix();
- 共轭/即反向旋转
一般不用inverse，在表示旋转的时候（范数是1），共轭即可表示相反的的旋转。
//q.inverse();
q.conjugate();
- 遍历元素
cout<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<endl;
线性插值Slerp
球面线性插值Slerp是一种在四元数空间中执行插值的技术，用于在两个四元数之间进行平滑的旋转过渡。在Eigen中，四元数类（Quaternion）提供了slerp方法，用于在两个四元数之间进行球面线性插值。
下面介绍一下slerp的参数和返回值：
template<typename OtherDerived>
Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;
参数：
- t:插值系数，范围为[0, 1]，表示返回四元数在两个四元数之间的位置，t=0表示返回当前四元数，t=1表示返回目标四元数。
- other：另一个四元数，作为插值的目标。
- 返回值：返回四元数，在当前四元数和目标四元数之间执行球面线性插值的结果。
代码示例：
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;
变换矩阵
1、Eigen::Isometry3d构造变换矩阵
1.1.对各个元素赋值
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.通过旋转矩阵和平移向量
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 );
注意不能直接变换矩阵赋值,像这样子会报错
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构造变换矩阵
对每一个元素赋值的方法是可行的的,我这里采用的是按矩阵块赋值
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、变换矩阵转成四元数+平移向量
使用Eigen::Quaterniond来构造四元数
Eigen::Quaterniond q(mat.block<3,3>(0,0));
得到平移向量
Eigen::Vector3d translation(mat.block<3,1>(0,3));
创建矩阵类型vector容器
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;
}
提取元素操作
对一个Eigen::Matrix3f类型的数据x，在想提取其第三行第一列元素时：
- 使用
x.row(2).col(0)操作并赋值给一个变量，提示不能将Eigen…类型值赋值过去； - 使用
x(2,0)操作可以完成提出元素并赋值。
在Eigen中，可以使用矩阵的列块来获取最右边一列。对于4*4的矩阵，可以使用matrix.col(3)来获取最右边一列。如果要将其转换为Vector3d类型，可以使用matrix.col(3).head<3>()来获取前三个元素。
Matrix4d matrix;
// ... Initialize matrix
Vector3d right_column = matrix.col(3).head<3>();
Reductions
maxCoeff(),minCoeff()
在使用maxCoeff()和minCoeff()函数是，可以寻求最大元素和最小元素，但如果我们想返回其位置，则需要给定相关参数，而参数的类型我们要使用Index类型，例如：
colwise(), rowwise()
Reductions还可以部分使用，即返回的不是一个值，而是一组值，可以看做是一种降维操作，所使用的函数为colwise(), rowwise()。例如:
Mat.colwise()理解为分别去看矩阵的每一列，然后再作用maxCoeff()函数，即求每一列的最大值。需要注意的是，colwise返回的是一个行向量（列方向降维），rowwise返回的是一个列向量（行方向降维）。
Eigen中norm、normalize、normalized的区别
norm()
对于Vector，norm返回的是向量的二范数，即
例如：
Vector2d vec(3.0,4.0);
cout << vec.norm() << endl; //输出5
对于Matrix，norm返回的是矩阵的弗罗贝尼乌斯范数（Frobenius Norm），即
例如：
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()
清楚了norm()的定义后，normalize()其实就是把自身的各元素除以它的范数。返回值为void。
例如：
vec.normalize();
cout << vec << endl; //输出： 0.6
// 0.8
mat.normalize(); //mat各元素除以mat.norm()
cout << mat << endl;
normalized()
而normalized()与normalize()类似，只不过normalize()是对自身上做修改，而normalized()返回的是一个新的Vector/Matrix，并不改变原有的矩阵。
四元数、欧拉角、旋转矩阵、旋转向量之间的转换
旋转向量
1.0 初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
1.1 旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();
1.2 旋转向量转欧拉角(X-Y-Z，即RPY)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);
1.3 旋转向量转四元数
Eigen::Quaterniond quaternion(rotation_vector);
//or
Eigen::Quaterniond quaternion;
quaternion=rotation_vector;
四元数
四元数转欧拉角(X-Y-Z，即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
欧拉角
旋转矩阵转欧拉角
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 在矩阵中指定的位置加值
在Eigen中，可以使用 operator() 方法访问矩阵中的元素，并且可以使用赋值运算符直接在该位置上加上指定的值。以下是一个简单的示例：
Eigen::MatrixXd mat(2, 2);
mat << 1, 2,
3, 4;
// 在第一行第二列的位置上加上3
mat(0, 1) += 3;
内存对齐的问题
在Eigen库的使用过程在经常出现类似这样的问题：
/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.
原因是因为Eigen库使用了SSE加速，需要按照128位进行对齐，这导致了Fixed-size vectorizable Eigen objects必须是16字节对齐，一般Eigen已经做好了对齐，例如Eigen库重载了new操作，然而有些情况这写对齐设置被覆盖了，造成了上述断言错误。
-
如果你不需要Eigen的加速，使用以下三种方法，可以解决这类错误：
使用不对齐对象：Eigen中对象提供了多种构造函数，使用不对齐的构造函数构造对象。使用Eigen_DONT_VECTORIZE宏：这禁止了所有16字节静态对齐代码 同时使用Eigen_DONT_VECTORIZE和EIGEN_DISABLE_UNALIGNED_ASSERT宏：这保留16字节对代码，但禁止了向量化 -
否则，根据以下情况：
-
程序中有类包含Eigen对象，例如：
classFoo{
Eigen::Vector2d v; //Fixed-size vectorizable Eigen objects
};
Foo *foo =
new Foo;
改为
classFoo{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //此处添加宏定义
Eigen::Vector2d v; //Fixed-size vectorizable Eigen objects
};
Foo *foo =
new Foo;
- STL容器或者手动内存分配
#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报错:Invalid sizes when resizing a matrix or array."’ failed 可能的一个原因
在用g2o求解非线性优化问题的时候的一个运行时报错信息
/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.
就是g2o::BlockSolverTraits的维度可能写错了。
template <int _PoseDim, int _LandmarkDim>
struct BlockSolverTraits
上面这个是g2o里面BlockSolverTraits的定义，PoseDim可以粗略理解成被优化节点的维度，LandmarkDim可以粗略理解成误差项的维度，
或者也可以改用下面这个类,这样就不用管维度的问题了，它会在运行时自己推算。
template <>
struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>