C++ Eigen库在3D相机手眼标定中的应用

正文

背景

Eigen 是基于C++开发代数的一个模板库：矩阵、矢量、数值解算器和相关算法。相比较Matlab，优势是利于c++开发，劣势是语法较复杂

环境配置

1.下载eigen源码包，可解压到任意位置

2.新建vc++工程，项目属性 -> C/C++ -> 常规 -> 附加包含目录 -> 编辑 -> 新建路径 -> 选择eigen文件夹所在路径 -> 运行下面的demo

Demo代码

https://www.cnblogs.com/winslam/p/12765822.html

Matrix类介绍

Matrix类模板6个参数

//共6个

Matrix

int RowsAtCompileTime,

int ColsAtCompileTime,

int Options = 0, //默认是按列存储数据，可改成按行

int MaxRowsAtCompileTime = RowsAtCompileTime, //行数上限

int MaxColsAtCompileTime = ColsAtCompileTime> //列数上限

Matrinx类模板前三个参数

数据类型，行数，列数，eigen已经定义好了常用的，规律很好找

Matrix

示例

typedef Matrix Matrix4f; //矩阵内存大小 float[16]

typedef Matrix Matrix3d; //矩阵内存大小 double[9]

特殊的Matrix类：Vector向量

是特殊的Matrix类，只有一行或一列，定义如下

typedef Matrix Vector3f; // 三行一列

typedef Matrix RowVector2i;// 一行两列

动态创建Matrix类对象

typedef Matrix MatrixXd; //行列都可不指定长度

typedef Matrix VectorXi;// 行动态

Matrix; //列动态

Matrix类构造函数

默认构造函数不会动态分配内存

对矩阵传参总是优先传行数

MatrixXf a(3,2); //3行2列

 而给向量传参 = 给向量传大小：

Vector2d a(5.0, 6.0);

Vector3d b(5.0, 6.0, 7.0);

Vector4d c(5.0, 6.0, 7.0, 8.0);

访问/初始化 Matrix元素

Matrix数据存储顺序，总是先列后行

逗号初始化，总是先行后列，但数据存储顺序还是不变

Vector无所谓，使用.transform()方法，即可转换行列

尽量用固定大小的Matrix，内存机制没深究

Matrix&Vector 运算

加减法

原则很简单，相同行列才能运算：

binary operator + as in a+b

binary operator - as in a-b

unary operator - as in -a

compound operator += as in a+=b

compound operator -= as in a-=b

注意：运算符已被重载，不可2个以上的矩阵同时参与运算。此时用遍历矩阵索引

VectorXf a(50),b(50),c(50),d(50);

a = 3*b + 4*c + 5*d;

改成

for(int i = 0;i < 50; + + i){

a[i] = 3*b[i] + 4*c[i] + 5*d[i];}

系数乘除法

binary operator * as in matrix*scalar

binary operator * as in scalar*matrix

binary operator / as in matrix/scalar

compound operator *= as in matrix*=scalar

compound operator /= as in matrix/=scalar

Matrix 转置，共轭，逆

//n阶正交矩阵a特性： a*=aT,无共轭

Eigen::MatrixXd t = Eigen::MatrixXd::Random(3,3);

cout << t.transpose() << endl; //a的转置

cout << t.conjugate() << endl; //a的共轭

cout << t.adjoint() << endl;   //a的逆

Matrix 乘法

代码

#include

#include

int main()

{

    using namespace Eigen;

    Matrix2d mat;mat << 1, 2, 3, 4;//运算符重载，矩阵按行接收数据，但是储存机制依旧优先按列，即mat经过上述赋值后为 [1,2

    //                   3,4]，但取值时，mat(1,0)值为3，mat(0,1)值为2

    Vector2d v1(-1, 1), v2(2, 0); //默认列向量

    cout << "mat * v1:" << endl<< mat * v1 << endl; cout << endl;

    cout << "mat * v2:" << endl << mat * v2 << endl; cout << endl;

    cout << "mat * mat:" << endl << mat * mat << endl;

}

输出

mat * v1:

1

1

mat * v2:

2

6

mat * mat:

 7 10

15 22

Vector的点乘和叉乘

点乘。纯代数运算，适用与任意长度的向量，前提是2个向量长度相等

/*

计算公式:

a(a1,a2,a3) , b(b1,b2,b3)

a·b = a1*b1 + a2*b2 + a3*b3

几何意义:

a·b =|a|*|b|*cosθ

*/

叉乘。用于空间几何，所以只适用于长度为3的向量

/*

几何意义

axb =|a|*|b|*sinθ

注意：叉乘结果是向量，方向在z轴上，θ表示向量a到向量b的角度，右手法则（从a到b）确定z朝向

*/

代码

#include

#include

int main()

{

    using namespace Eigen;

Vector3d v1(1, 2, 3);

    Vector3d v2(1, 1, 2);

    //点积方法1：用同维度的向量做点积

    cout << "v1 点乘 v2 :" << endl << v.dot(v2) << endl;

    //点积方法2：向量转化为矩阵乘积来做点积

    cout << "v1逆 点乘 v2 :" << endl << v.adjoint() * w << endl;

    

    //叉乘，外积

    cout << "v1 叉乘 v2 :" << endl << v.cross(v2) << endl;

    return 1;

}

输出

v1 点乘 v2 :

9

v1 叉乘 v2 :

 1

 1

-1

v1逆 点乘 v2 :

9

Matrix内部数据算术

代码

#include

#include

using namespace std;

int main()

{

  Eigen::Matrix2d mat;

  mat << 1, 2,

         3, 4;

  cout << "Here is mat.sum():       " << mat.sum()       << endl;//求和

  cout << "Here is mat.prod():      " << mat.prod()      << endl;//连乘

  cout << "Here is mat.mean():      " << mat.mean()      << endl;//均值

  cout << "Here is mat.minCoeff():  " << mat.minCoeff()  << endl;//最小值

  cout << "Here is mat.maxCoeff():  " << mat.maxCoeff()  << endl;//最大值

  //主对角线系数和

  cout << "Here is mat.trace():     " << mat.trace()     << endl;

  //某些函数可以重载

  Matrix3f m = Matrix3f::Random();

  std::ptrdiff_t i, j;

  float minOfM = m.minCoeff(&i,&j);

  cout << "Here is the matrix m:\n" << m << endl;

  cout << "Its minimum coefficient (" << minOfM 

       << ") is at position (" << i << "," << j << ")\n\n";

 

  RowVector4i v = RowVector4i::Random();

  int maxOfV = v.maxCoeff(&i);

  cout << "Here is the vector v: " << v << endl;

  cout << "Its maximum coefficient (" << maxOfV 

       << ") is at position " << i << endl;

}

输出

Here is mat.sum():       10

Here is mat.prod():      24

Here is mat.mean():      2.5

Here is mat.minCoeff():  1

Here is mat.maxCoeff():  4

Here is mat.trace():     5

Here is the matrix m:

     -1 -0.0827  -0.906

 -0.737  0.0655   0.358

  0.511  -0.562   0.359

Its minimum coefficient (-1) is at position (0,0)

Here is the vector v:  9 -2  0  7

Its maximum coefficient (9) is at position 0

Matrix类与Array类互换

矩阵运算用矩阵类，系数运算用数组类，互相转换用.matrix()方法 和 .array()方法

求方阵的行列式值

Tatrix3d mat;mat << 1,2,3,4,5,6,7,8,9; //3阶方阵

double result = matrix.determinant();

Matrix初始化方法

前面提到过的逗号初始化

.Zero()方法初始化所有系数为0

.Random()方法用随机系数填充矩阵或数组

.Identity()方法初始化一个单位矩阵

用分块矩阵构造成一个大矩阵

代码

const int size = 6;

MatrixXd mat1(size, size);

mat1.topLeftCorner(size/2, size/2)     = MatrixXd::Zero(size/2, size/2);

mat1.topRightCorner(size/2, size/2)    = MatrixXd::Identity(size/2, size/2);

mat1.bottomLeftCorner(size/2, size/2)  = MatrixXd::Identity(size/2, size/2);

mat1.bottomRightCorner(size/2, size/2) = MatrixXd::Zero(size/2, size/2);

std::cout << mat1 << std::endl << std::endl;

 

MatrixXd mat2(size, size);

mat2.topLeftCorner(size/2, size/2).setZero();

mat2.topRightCorner(size/2, size/2).setIdentity();

mat2.bottomLeftCorner(size/2, size/2).setIdentity();

mat2.bottomRightCorner(size/2, size/2).setZero();

std::cout << mat2 << std::endl << std::endl;

 

MatrixXd mat3(size, size);

mat3 << MatrixXd::Zero(size/2, size/2), MatrixXd::Identity(size/2, size/2),

        MatrixXd::Identity(size/2, size/2), MatrixXd::Zero(size/2, size/2);

std::cout << mat3 << std::endl;

输出

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

1 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

1 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

1 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 0

构造齐次矩阵（4*4）

旋转向量，旋转矩阵，欧拉角，四元数4者互换

https://blog.csdn.net/yang__jing/article/details/82316093?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.control

四元数注意点

输入顺序是[w,x,y,z] ，其中w是实数部分

储存和输出顺序是[x,y,z,w] ，其中w是实数部分

互为相反数的2组四元数效果一样

欧拉角注意点

pose转matrix默认用'gba'形式（ R = Rx * Ry * Rz），T1.rotation().eulerAngles(0, 1, 2) 

线性代数求解Ax=b形式方程

QR分解法：

代码

#include

#include

 

using namespace std;

using namespace Eigen;

 

int main()

{

   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;

}

输出

Here is the matrix A:

 1  2  3

 4  5  6

 7  8 10

Here is the vector b:

3

3

4

The solution is:

-2

 1

 1

bdcSVD分解法

最精确但速度最慢的求解方法，用于求解线性方程，在没有解的情况下能逼近

代码

#include

#include

 

using namespace std;

using namespace Eigen;

 

int main()

{

   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;

}

您可以通过我们的官方网站了解更多的产品信息，或直接来电咨询4006-888-532。