为了将Matlab写的运动学程序转化为C++所编写的dll,需要用用到矩阵库Eigen,Eigen库是一个使用C++源码编写的矩阵库,基本上能满足计算中所需要用到的运算,下面介绍一些库的入门学习。
1.首先是关于固定大小矩阵,向量的定义,初始化
#include<iostream>
#include<Eigen/Core>
using namespace std;
using namespace Eigen;
//import most commen Eigen types
int main(int, char *[])
{
Matrix3fm3; //3X3单精度矩阵
m3<< 1, 2, 3, 4, 5, 6, 7, 8, 9;
Matrix4fm4 = Matrix4f::Identity();//4X4单位矩阵(单精度)
Vector4iv4(1, 2, 3, 4); //长度为4的整形向量
//输出结果
std::cout<< "m3\n" << m3 << "\nm4:\n"
<<m4 << "\nv4:\n" << v4 << std::endl;
getchar();
return0;
}
/*学习笔记*/
/*
Matrix表示矩阵, Vector表示向量, 数字表示维度,最后f和i分别表示单精度和整形数据类型
固定大小表示编译时,行数和列数是固定的,这时,Eigen不会分配动态内存。这对于比较小的矩阵比较合适,例如16*16
*/
2.动态定义矩阵Matrix,向量Vector
#include<iostream>
#include<Eigen/Core>
using namespace std;
using namespace Eigen;
int main(int, char *[])
{
for(int size = 1; size < 4; ++size)
{
MatrixXim(size, size + 1); //一个整形大小为(size)X(size+1)的矩阵
for(int j = 0; j < m.cols(); ++j)//遍历列
for(int i = 0; i < m.rows(); ++i)//遍历行
m(i,j) = i + j*m.rows();//使用圆括号m(i, j)访问矩阵的元素
std::cout<< m << "\n\n";//打印矩阵
}
//动态向量
VectorXfv(4); //定义一个4维单精度向量
//使用圆括号或方括号[]访问向量元素
v[0]= 1; v[1] = 2; v(2) = 3; v(3) = 4;
std::cout<< "\nv:\n" << v << endl;
getchar();
return0;
}
//学习心得
/*
X表示动态大小
#include<Eigen/Eigen>将包含所有的Eigen函数。#include<Eigen/Dense>包含所有普通矩阵函数,不包含稀疏矩阵函数,他们会增加编译时间。
*/
3.使用Zero, Ones,UnitX,Constant初始化矩阵
#include<iostream>
#include<Eigen\Core>
using namespace std;
using namespace Eigen;
int main(int, char*[])
{
intsize = 3;
floatvalue = 3.0f;
VectorXfx; //定义动态向量
x= VectorXf::Zero(size); //全0向量
cout<< x << endl << endl;
x= VectorXf::Ones(size); //全1向量
//创建固定大小的基向量
Vector3fy;
y= Vector3f::UnitX(); //1 0 0
cout<< y << endl << endl;
y= Vector3f::UnitY(); //0 1 0
cout<< y << endl << endl;
y= Vector3f::UnitZ(); //0 0 1
cout<< "创建动态大小的基向量" << endl;
VectorXfz;
z= VectorXf::Unit(4, 2);
cout<< z << endl << endl;
z= Vector4f(0, 1, 0, 0);
cout<< z << endl << endl;
z= Vector4f::UnitY();
cout<< z << endl << endl;
getchar();
return0;
}
#include<iostream>
#include<Eigen/Core>
using namespace std;
using namespace Eigen;
int main(int, char*[])
{
floatvalue = 3.0f;
introws = 3;
intcols = 4;
MatrixXfx;
x= MatrixXf::Zero(rows, cols);
cout<< x << endl << endl;
x= MatrixXf::Ones(rows, cols);
cout<< x << endl << endl;
x= MatrixXf::Constant(rows, cols, value);//constant:不变的恒定的
cout<< x << endl << endl;
x= MatrixXf::Identity(rows, cols);
cout<< x << endl;
getchar();
return0;
}
4.通过cast转换数据类型
#include<iostream>
#include<Eigen\Core>
using namespace std;
using namespace Eigen;
//元素通过Matrix::cast()自动转换
int main(int, char*[])
{
Vector3dmd(1, 2, 3);
Vector3fmf = md.cast<float>();
cout<< "md = " << md << endl;
cout<< "mf = " << mf << endl;
getchar();
return0;
}
5.使用逗号初始化矩阵
#include<iostream>
#include<Eigen\Core>
using namespace std;
using namespace Eigen;
int main(int, char *[])
{
Matrix3fm;
m<< 1, 2, 3,
4,5, 6,
7,8, 9;
cout<< m << endl;
getchar();
return0;
}
//使用逗号初始化矩阵
6.创建固定大小的矩阵和向量
#include<iostream>
#include<Eigen/Core>
using namespace Eigen;
using namespace std;
int main(int, char *[])
{
floatvalue = 3.0;
Matrix3fx; //创建一个3x3的单精度矩阵
x= Matrix3f::Zero(); //全零矩阵
cout<< x << endl<<endl;
x= Matrix3f::Ones(); //全一矩阵
cout<< x << endl << endl;
x= Matrix3f::Constant(value);//全value矩阵
cout<< x << endl << endl;
x= Matrix3f::Identity(); //单位矩阵
cout<< x << endl << endl;
x= Matrix3f::Random(); //随机矩阵
cout<< x << endl << endl;
x.setZero(); //设置x全为0
cout<< x << endl << endl;
x.setOnes(); //设置x全为1
cout<< x << endl << endl;
x.setIdentity(); //设置x为单位矩阵
cout<< x << endl << endl;
x.setConstant(value); //设置x为全value矩阵
cout<< x << endl << endl;
x.setRandom(); //设置x为随机局长你
cout<< x << endl << endl;
getchar();
return0;
}
7. 矩阵的简单运算
#include<iostream>
#include<Eigen\Core>
using namespace std;
using namespace Eigen;
int main(int, char*[])
{
MatrixXfres(10, 10); //动态创建10x10的矩阵
Matrix3fa, b;
a= Matrix3f::Identity(); //单位矩阵
b= Matrix3f::Constant(3); //全3矩阵
res= a + b; //res is resized to size3x3
cout<< a << endl << endl;
cout<< b << endl << endl;
cout<< res << endl << endl;
getchar();
return0;
}
到这里,Eigen的基本的赋值,初始化操作已经完全结束了,打过一遍以上的程序,基本上就可以开始编写程序了,下面记录一下我在编写运动学算法的时候会用到的几个技巧:
1. 将(double)数组转换为Matirx矩阵
这里,我使用的时候一般是将double(16)转换为Matrix4d,具体用法如下
Map<Matrix4d>(dSTOut, 4, 4) = sTargetMatrix;
这里用到的是Map语句,其中dSTOut为16个元素的double数组,sTargetMatrix为Matrix4d的矩阵,需要注意的是,这里的dSTOut必须是按列排列,才能将矩阵还原
2. 将Matrix4d转换为(double)数组
和1中所述类似,这里我一般也是将Matrix4d转换为按列排列的double(16)数组,具体用法如下:
dD_Tm1 = Map<Matrix4d>(dDArmOut_Tm1, 4, 4);
其中dD_Tm1为Matrix4d矩阵,dDArmOut_Tm1为按列排列的16元素double数组,采用此语句即可将Matrix4d转换为按列排列的double数组。
3.在对矩阵进行求逆的运算中,矩阵的逆有可能不存在,为了防止程序崩溃,通常这样写:
BoolbInvertible = false;
Matirx4dT1 = Matrix4d::zero();
T1.computeInverseWithCheck(inverseT1,bInvertible);
if(false == bInvertible)
{
return1;
}
traMat_a1= inverseT1 * dTargetMatrix;
4. 获取矩阵的子矩阵,这种方法在矩阵运算中也经常会用到,之前也查过很多博客,发现其中好多所说的取矩阵方式有有一些问题
uniVec_P1= traMat_a1.block(0, 3, 1, 3);
这是我所使用的方法,才用block方式进行取子矩阵的操作,这里block中有4个参数,前两个为取在矩阵中取子矩阵的首个元素的位置,这里的0, 3代表第1行,第4列,后两个元素分别代表从首个元素开始,所要获取的行数和列数,这里的1,3代表获取每行获取一个元素,总共获取三列。
5. 将按行排列的数组转换为按列排列的数组
这个方法也适用于将按列排列的数组转换为按行排列的数组
intMatrixTran(double *dInMat)
{
double dTS[16] = {0.0};
int itran = 0;
int itrannum = 0;
for(int a = 0; a < 16; a ++)
{
dTS[a] = dInMat[a];
}
for(int j = 0; j < 4; j++)
{
itrannum = j;
for(int i = 0; i < 4; i++)
{
dInMat[itran] =dTS[itrannum];
itrannum += 4;
itran++;
}
}
return 0;
}
6.按照SVD的方法求矩阵伪逆
//使用svd方式求伪逆
template<typename_Matrix_Type_>
_Matrix_Type_pseudoInverse(const_Matrix_Type_&a,oublepsilon=std::numeric_limits<double>::epsilon())
{
Eigen::JacobiSVD< _Matrix_Type_ > svd(a,Eigen::ComputeThinU | Eigen::ComputeThinV);
double tolerance = epsilon * std::max(a.cols(),a.rows()) *svd.singularValues().array().abs()(0);
return svd.matrixV() * (svd.singularValues().array().abs() >tolerance).select(svd.singularValues().array().inverse(),0).matrix().asDiagonal() * svd.matrixU().adjoint();
}
在Eigen库中,没有直接像matlab中pinv那样求伪逆的程序,但是可以通过SVD方式,求出,方法如上所示,具体没有细致研究程序如何运行,有时间了再详细看这个吧,总的来说,求伪逆可以算是将matlab所写的运动学程序转换为c++时,最难的一个地方了
总结
虽然最后还可能需要大量的时间,对所写的程序进行调试,但是主要工作都已经完成,这次的编程实践,对于我这边来说可以定性为简单,因为并不需要我对运动学正逆解进行大量的思考,只需要完全按照黄康所写的matlab程序,转写为c语言程序,可算做c语言吧,因为并没有涉及到类的撰写,不过,心中还是有一些冲动,想过要将整个程序转换为类的样式,希望所系的运动学算法可以适用于多台设备,而不是仅仅这一种台车,发现了对于编程人员来说,要对所写的程序有本质的了解,对问题的特征可以进行提炼,从程序一开始,就要对程序进行全局的考虑,否则就会后患无穷,对后期工作造成很坏的影响,就拿这次来说,如果我之前按照黄康那种方式进行程序的撰写,那么在后期和王工进行接口对接的时候,就会面临大量程序接口以及程序内容的修改;其次,发现了程序统一规划的重要性,以及注释的重要性,目前程序的行数到达三千多行,发现自己对整个程序的掌控力已经在慢慢的失去,在开始变成之前,一定要统一接口,统一输入输出参数;另外,还有对于VB.Net的小部分理解,可以说VB.Net是一种很容易上手的语言,并不想c语言和c++那样需要严格的整体格式,感觉编译器在进行编译的时候会做很多事;想完成什么功能,只需要在其中写好相应的function后者sub即可,让我感觉,VB.Net本身就是一个大的累,我们所做的工作只是在类中添加所需要的函数而已。