为了将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本身就是一个大的累，我们所做的工作只是在类中添加所需要的函数而已。
