Eigen库在ROS中的使用例程

Eigen库在ROS中的使用例程

代码参考链接

// 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(int argc, char** argv) {
    ros::init(argc, argv, "example_eigen_plane_fit"); //node name
    ros::NodeHandle nh; // create a node handle; need to pass this to the class constructor

    ros::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 origin
    Eigen::Vector3d normal_vec(1,2,3); // here is an arbitrary normal vector, initialized to (1,2,3) upon instantiation
    ROS_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 column
    normal_vec/=normal_vec.norm(); // make this vector unit length
    cout<<"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 normal
    cout<<"plane distance from origin: "<<dist<<endl;

    // to generate points on the plane, construct a pair of axes perpendicular to the plane normal
    Eigen::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 method
    Rot_z.row(1)<<1, 0,0;  //second row
    Rot_z.row(2)<<0, 0,1;  // yada, yada
    cout<<"Rot_z: "<<endl;  

    cout<<Rot_z<<endl;  // Eigen matrices and vectors are nicely formatted; better: use ROS_INFO_STREAM() instead of cout

    ROS_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 bo