ROS中使用Eigen库[不定期更新]

前期说明

ROS中的数据操作需要线性代数，Eigen库是C++中的线性代数计算库。
Eigen库独立于ROS，但是在ROS中可以使用。
Eigen库可以参见http://eigen.tuxfamily.org

在ROS中配置Eigen

在CMakeLists.txt文件中要做以下配置

#uncomment the following 4 lines to use the Eigen library
find_package(cmake_modules REQUIRED)
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
add_definitions(${EIGEN_DEFINITIONS})

源码中要包含以下文件:

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

如果需要使用其他的功能，可以包含更多的头文件。
参见这个网址http://eigen.tuxfamily.org/dox/group__QuickRefPage.html#QuickRef_Headers

简单实例

定义一个向量：

// here is an arbitrary normal vector
// initialized to (1,2,3) upon instantiation
Eigen::Vector3d normal_vec(1,2,3);

将实例化一个Eigen对象,它是由3个双精度值组成的列向量. 该对象命名为normal_vec,初始化值为(1,2,3).

将向量变成单位向量

// make this vector unit length
normal_vec/=normal_vec.norm();

这里使用了Eigen::Vector3d的一个成员函数norm(),用以计算向量的欧几里得长度.如果normal_vec是一个非零向量,可以用norm()函数直接求其长度.

定义一个矩阵的形式：

可以按行赋值,一行一行地用数据生成矩阵。

Eigen::Matrix3d Rot_z;
Rot_z.row(0)<<0,-1, 0;  // populate the first row--shorthand method
Rot_z.row(1)<<1, 0, 0;  // second row
Rot_z.row(2)<<0, 0, 1;  // yada, yada
cout<<"Rot_z: "<<endl;

还有很多其他方法用于初始化或生成矩阵和向量.例如,用户可以用零来生成一个向量或矩阵.其中参数(3,1)指定了3行1列.(向量是特殊的矩阵,不是一行就是一列)

// first compute the centroid of the data:
// here's a handy way to initialize data to all zeros; more variants exist
// see http://eigen.tuxfamily.org/dox/AsciiQuickReference.txt
Eigen::Vector3d centroid = Eigen::MatrixXd::Zero(3,1);

用户可以在实例化的时候将初始值指定为构造函数的参数.将向量初始化为1:

//xxxx  one_vec*dist = point.dot(nx,ny,nz)
// so, one_vec = points_mat.transpose()*x_vec, where x_vec = [nx;ny;nz]/dist (does not work if dist=0)
// this is of the form: b = A*x, an overdetermined system of eqns
// solve this using one of many Eigen methods
// see: http://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
ROS_INFO("2ND APPROACH b = A*x SOLN");
Eigen::VectorXd  ones_vec= Eigen::MatrixXd::Ones(npts,1); // this is our "b" vector in b = A*x

矩阵与向量的乘法

//here is how to multiply a matrix times a vector
//although Rot_z and normal_vec are both objects (with associated data and member methods)
//multiplication is defined,
//resulting in the data members being altered or generated as expected for matrix-vector multiplies
v1 = Rot_z*normal_vec;

示例代码

// wsn: program to illustrate use of Eigen library to fit a plane to a collection of points#include<ros/ros.h>#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>using namespace std;
//using namespace Eigen; //if you get tired of typing Eigen:: everywhere, uncomment this.// but I'll leave this as required, for now, to highlight when Eigen classes are being useddouble g_noise_gain = 0.1; //0.1; //0.1; //0.1; //decide how much noise to add to points; start with 0.0, and should get precise resultsint main(int argc, char** argv) {ros::init(argc, argv, "example_eigen_plane_fit"); //node nameros::NodeHandle nh; // create a node handle; need to pass this to the class constructorros::Rate sleep_timer(1.0); //a timer for desired rate, e.g. 1Hz//xxxxxxxxxxxxxxxxxx  THIS PART IS JUST TO GENERATE DATA xxxxxxxxxxxxxxxxxxxx//xxxxxxxxxxxxxxxxxx  NORMALLY, THIS DATA WOULD COME FROM TOPICS OR FROM GOALS xxxxxxxxx// define a plane and generate some points on that plane// the plane can be defined in terms of a normal vector and a distance from the originEigen::Vector3d normal_vec(1,2,3); // here is an arbitrary normal vector, initialized to (1,2,3) upon instantiationROS_INFO("creating example noisy, planar data...");cout<<"normal: "<<normal_vec.transpose()<<endl; //.transpose() is so I can display the components on the same line, instead of as a columnnormal_vec/=normal_vec.norm(); // make this vector unit lengthcout<<"unit length normal: "<<normal_vec.transpose()<<endl;double dist = 1.23;  // define the plane to have this distance from the origin.  Note that if we care about positive/negative sides of a plane,// then this "distance" could be negative, as measured from the origin to the plane along the positive plane normalcout<<"plane distance from origin: "<<dist<<endl;// to generate points on the plane, construct a pair of axes perpendicular to the plane normalEigen::Vector3d v1,v2; //need to fill in values for these// we'll use an Eigen "Matrix" type to help out.  Define a 3x3 "double precision" matrix, Matrix3d//define a rotation about the z axis of 90 deg; elements of this matrix are:// [0,-1,0; //  1, 0, 0; //  0;0;1]Eigen::Matrix3d Rot_z;Rot_z.row(0)<<0,-1,0;  // populate the first row--shorthand methodRot_z.row(1)<<1, 0,0;  //second rowRot_z.row(2)<<0, 0,1;  // yada, yadacout<<"Rot_z: "<<endl;  cout<<Rot_z<<endl;  // Eigen matrices and vectors are nicely formatted; better: use ROS_INFO_STREAM() instead of coutROS_INFO_STREAM(endl<<Rot_z);// we need another vector to generate two desired vectors in our plane.// start by generating a new vector that is a rotation of our normal vector, rotated about the z-axis// this hack will NOT work if our normal_vec = [0,0,1], v1 = Rot_z*normal_vec; //here is how to multiply a matrix times a vector//although Rot_z and normal_vec are both objects (with associated data and member methods), multiplication is defined,// resulting in the data members being altered or generated as expected for matrix-vector multipliesROS_INFO_STREAM("v1: "<<v1.transpose()<<endl);// let's look at the dot product between v1 and normal_vec, using two approachesdouble dotprod = v1.dot(normal_vec); //using the "dot()" member functiondouble dotprod2 = v1.transpose()*normal_vec;// alt: turn v1 into a row vector, then multiply times normal_veccout<<"v1 dot normal: "<<dotprod<<"; v1.transpose()*normal_vec: "<<dotprod2<<endl; //yields the same answercout<<"(should be identical)"<<endl;// let's look at the cross product, v1 X normal_vec; use the member fnc cross()v2 = v1.cross(normal_vec);v2/=v2.norm(); // normalize the output, i.e. make v2 unit lengthcout<<"v2: "<<v2.transpose()<<endl;//because v2 was generated from the cross product of v1 and normal_vec, it should be perpendicular to both// i.e., the dot product with v1 or normal_vec should be = 0dotprod = v2.dot(normal_vec);cout<<"v2 dot normal_vec = "<<dotprod<<"  (should be zero)"<<endl;v1 = v2.cross(normal_vec);  // re-use v1; make it the cross product of v2 into normal_vec// thus, v1 should now also be perpendicular to normal_vec (and thus it is parallel to our plane)// and also perpendicular to v2 (so both v1 and v2 lie in the plane and are perpendicular to each other)cout<<"v1= "<<v1.transpose()<<endl;cout<<" v1 dot v2 = "<<v1.dot(v2)<<"; v1 dot normal_vec = "<<v1.dot(normal_vec)<<endl;cout<<"(these should also be zero)"<<endl;// we now have two orthogonal vectors, both perpendicular to our defined plane's normal// we'll use these to generate some points that lie in our defined plane:int npts= 10; // create this many planar pointsEigen::MatrixXd points_mat(3,npts);  // create a matrix, double-precision values, 3 rows and npts columns// we will populate this with 3-D points, column by columnEigen::Vector3d point; //a 3x1 vectorEigen::Vector2d rand_vec; //a 2x1 vector//generate random points that all lie on plane defined by distance and normal_vecfor (int ipt = 0;ipt<npts;ipt++) {// MatrixXd::Random returns uniform random numbers in the range (-1, 1).rand_vec.setRandom(2,1);  // populate 2x1 vector with random values//cout<<"rand_vec: "<<rand_vec.transpose()<<endl; //optionally, look at these random values//construct a random point ON the plane normal to normal_vec at distance "dist" from origin:// a point on the plane is a*x_vec + b*y_vec + c*z_vec, where we may choose// x_vec = v1, y_vec = v2 (both of which are parallel to our plane) and z_vec is the plane normal// choose coefficients a and b to be random numbers, but "c" must be the plane's distance from the origin, "dist"point =  rand_vec(0)*v1 + rand_vec(1)*v2 + dist*normal_vec;//save this point as the i'th column in the matrix "points_mat"points_mat.col(ipt) = point;}//all of the above points are identically on the plane defined by normal_vec and distcout<<"random points on plane (in columns): "<<endl; // display these points; only practical for relatively small number of pointscout<<points_mat<<endl;// add random noise to these points in range [-0.1,0.1]Eigen::MatrixXd Noise = Eigen::MatrixXd::Random(3,npts);cout<<"noise_gain = "<<g_noise_gain<<"; edit this as desired"<<endl;// add two matrices, term by term.  Also, scale all points in a matrix by a scalar: Noise*g_noise_gainpoints_mat = points_mat + Noise*g_noise_gain;  cout<<"random points on plane (in columns) w/ noise: "<<endl;cout<<points_mat<<endl;//xxxxxxxxxxxxxxxxxx  DONE CREATING PLANAR DATA xxxxxxxxxxxxxxxxxx// xxxxxxxxxxxxxxx   NOW, INTERPRET THE DATA TO DISCOVER THE UNDERLYING PLANE xxxxxxxx//now let's see if we can discover the plane from the data:cout<<endl<<endl;ROS_INFO("starting identification of plane from data: ");// first compute the centroid of the data:// here's a handy way to initialize data to all zeros; more variants exist// see http://eigen.tuxfamily.org/dox/AsciiQuickReference.txtEigen::Vector3d centroid = Eigen::MatrixXd::Zero(3,1);//add all the points together:npts = points_mat.cols(); // number of points = number of columns in matrix; check the sizecout<<"matrix has ncols = "<<npts<<endl;for (int ipt =0;ipt<npts;ipt++) {centroid+= points_mat.col(ipt); //add all the column vectors together}centroid/=npts; //divide by the number of points to get the centroidcout<<"centroid: "<<centroid.transpose()<<endl;// subtract this centroid from all points in points_mat:Eigen::MatrixXd points_offset_mat = points_mat;for (int ipt =0;ipt<npts;ipt++) {points_offset_mat.col(ipt)  = points_offset_mat.col(ipt)-centroid;}//compute the covariance matrix w/rt x,y,z:Eigen::Matrix3d CoVar;CoVar = points_offset_mat*(points_offset_mat.transpose());  //3xN matrix times Nx3 matrix is 3x3cout<<"covariance: "<<endl;cout<<CoVar<<endl;// here is a more complex object: a solver for eigenvalues/eigenvectors;// we will initialize it with our covariance matrix, which will induce computing eval/evec pairsEigen::EigenSolver<Eigen::Matrix3d> es3d(CoVar);Eigen::VectorXd evals; //we'll extract the eigenvalues to here//cout<<"size of evals: "<<es3d.eigenvalues().size()<<endl;//cout<<"rows,cols = "<<es3d.eigenvalues().rows()<<", "<<es3d.eigenvalues().cols()<<endl;cout << "The eigenvalues of CoVar are:" << endl << es3d.eigenvalues().transpose() << endl;cout<<"(these should be real numbers, and one of them should be zero)"<<endl;cout << "The matrix of eigenvectors, V, is:" << endl;cout<< es3d.eigenvectors() << endl << endl;cout<< "(these should be real-valued vectors)"<<endl;// in general, the eigenvalues/eigenvectors can be complex numbers//however, since our matrix is self-adjoint (symmetric, positive semi-definite), we expect// real-valued evals/evecs;  we'll need to strip off the real parts of the solutionevals= es3d.eigenvalues().real(); // grab just the real partscout<<"real parts of evals: "<<evals.transpose()<<endl;// our solution should correspond to an e-val of zero, which will be the minimum eval//  (all other evals for the covariance matrix will be >0)// however, the solution does not order the evals, so we'll have to find the one of interest ourselvesdouble min_lambda = evals[0]; //initialize the hunt for min evalEigen::Vector3cd complex_vec; // here is a 3x1 vector of double-precision, complex numbersEigen::Vector3d est_plane_normal;complex_vec=es3d.eigenvectors().col(0); // here's the first e-vec, corresponding to first e-val//cout<<"complex_vec: "<<endl;//cout<<complex_vec<<endl;est_plane_normal = complex_vec.real();  //strip off the real part//cout<<"real part: "<<est_plane_normal.transpose()<<endl;//est_plane_normal = es3d.eigenvectors().col(0).real(); // evecs in columnsdouble lambda_test;int i_normal=0;//loop through "all" ("both", in this 3-D case) the rest of the solns, seeking min e-valfor (int ivec=1;ivec<3;ivec++) {lambda_test = evals[ivec];if (lambda_test<min_lambda) {min_lambda =lambda_test;i_normal= ivec; //this index is closer to index of min evalest_plane_normal = es3d.eigenvectors().col(ivec).real();}}// at this point, we have the minimum eval in "min_lambda", and the plane normal// (corresponding evec) in "est_plane_normal"/// these correspond to the ith entry of i_normalcout<<"min eval is "<<min_lambda<<", corresponding to component "<<i_normal<<endl;cout<<"corresponding evec (est plane normal): "<<est_plane_normal.transpose()<<endl;cout<<"correct answer is: "<<normal_vec.transpose()<<endl;double est_dist = est_plane_normal.dot(centroid);cout<<"est plane distance from origin = "<<est_dist<<endl;cout<<"correct answer is: "<<dist<<endl;cout<<endl<<endl;//xxxx  one_vec*dist = point.dot(nx,ny,nz)// so, one_vec = points_mat.transpose()*x_vec, where x_vec = [nx;ny;nz]/dist (does not work if dist=0)// this is of the form: b = A*x, an overdetermined system of eqns// solve this using one of many Eigen methods// see: http://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.htmlROS_INFO("2ND APPROACH b = A*x SOLN");Eigen::VectorXd  ones_vec= Eigen::MatrixXd::Ones(npts,1); // this is our "b" vector in b = A*xEigen::MatrixXd A = points_mat.transpose(); // make this a Nx3 matrix, where points are along the rows// we'll pick the "full pivot LU" solution approach; see: http://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html// a matrix in "Eigen" has member functions that include solution methods to this common problem, b = A*x// use: x = A.solution_method(b)Eigen::Vector3d x_soln = A.fullPivLu().solve(ones_vec);//cout<<"x_soln: "<<x_soln.transpose()<<endl;double dist_est2 = 1.0/x_soln.norm();x_soln*=dist_est2;cout<<"normal vec, 2nd approach: "<<x_soln.transpose()<<endl;cout<<"plane distance = "<<dist_est2<<endl;return 0;// while (ros::ok()) {//     sleep_timer.sleep();// }
}

运行结果:

ROS中使用Eigen库[不定期更新]相关推荐

JavaScript中的小陷阱(不定期更新。。)
1. var scores = [1, 2, 3]; var total = 0; for (var score in scores) {total += score; }var mean = tot ...
工作中常见问题汇总（不定期更新）
基于工作中遇到的一些问题,总结一下经验: 单双引号的区别大家都知道 php 里双引号可以解析变量,单引号不可以,其实换行符等特殊情况也需要双引号处理. $aWhiteIP = explode('\n ...
关于C++中Eigen库效率提升的思考
目录引言一.什么是Eigen? 二.使用步骤 1.引入库 2.建立矩阵 3.基本操作三.具体的例子--矩阵乘法 1.Eigen库 2.GPU并行计算总结可能的方案引言在处理矩阵运算上,各 ...
【不定期更新】游戏开发中的一些良好习惯与技术技巧
平时programing时想到和积累的一些小技巧,在这里写成一篇日志,欢迎拍砖. <技巧一> 使用二进制位移运算来进行乘数是2的幂的简单整数乘法(除法),因为所有的数据在计算机中都是以二进 ...
ROS中的tf与Eigen的转换
写ROS程序时会经常遇到tf与Eigen库的转换,即算法中大多会使用Eigen来进行运算和表示机器人的位姿,但是最终需要tf将pose发布出去,所以需要将Eigen表示的pose转换为tf以及相应的m ...
收集优质的中文前端博客（不定期更新中）
收集优质的中文前端博客(不定期更新中) 注:博主的公司信息来自网上公开资料,仅供参考,不保证准确性. 个人(控制在42个以内) 阮一峰的网络日志(蚂蚁金服) <读懂 ECMAScript 规格& ...
FFTW、Eigen库在VisualStudio中的导入和使用
文章目录前言一.Eigen 1.Eigen获取 2.Eigen导入二.FFTW 1.FFTW3获取 2.FFTW3初始化 3.FFTW3导入三.测试总结前言 FFTW3下载链接:http: ...
C++ Eigen 库中旋转向量、旋转矩阵、欧拉角、四元数的定义及互相转换
今天看师兄写的使用力反馈设备操作机械臂的代码,里边涉及到了Eigen 库中的旋转变换,表征旋转变换的有旋转向量Eigen::AngleAxisd.欧拉角Eigen::Vector3d.旋转矩阵Eige ...
【ROS学习】- tf学习 - tf中重要函数解析 (陆续更新....)
文章目录一.函数waitForTransform().lookupTransform() 具体解释二.函数 tf::StampedTransform().sendTransform() 具体解释 ...