首页  技术文章  C++ Eigen库在3D相机手眼标定中的应用

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

发布时间:2021-03-02 10:32:25 浏览量:78 作者:Criket

摘要

Eigen 是基于C++开发代数的一个模板库:矩阵、矢量、数值解算器和相关算法。相比较Matlab,优势是利于基于c++的3D相机开发

(大部分3D相机SDK都支持c++),劣势是语法较复杂。本文目标是针对3D相机手眼标定过程中有关矩阵算术的eigen库运用进行学习。

正文


背景

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