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

简单实例

1. 定义一个向量：

// 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).

2. 将向量变成单位向量

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

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

3. 定义一个矩阵的形式：

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

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

4. 矩阵与向量的乘法

//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 used
    double 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 results

int main(