Eigen demo与文件读写 汇总

Ubuntu 14.04

1. eigen的相关实例代码

/home/yake/catkin_ws/src/handeye_calib_camodocal/src/eigen_demo.cpp

修改hand eye calibration的时候，本来是tf转换为eigen矩阵，现在经由xyz rpy转到旋转矩阵

http://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html

#include <iostream>#include <Eigen/Dense>using namespace Eigen;using namespace std;int main(){  MatrixXd m(2,2);  m(0,0) = 1;  m(1,0) = 2;  m(0,1) = 3;  m(1,1) = m(1,0) + m(0,1);  std::cout << m << std::endl;//  1 3//  2 5  m.resize (3,3);  m <<   1, 2, 3,             4, 5, 6,             7, 8, 9;  std::cout << m<<std::endl;  std::cout<<MatrixXd::Constant(3, 3, 1.2)<<std::endl;//  1.2 1.2 1.2//  1.2 1.2 1.2//  1.2 1.2 1.2   std::cout<<MatrixXd::Constant(3, 3, 1.2)* 50<<std::endl;    Affine3d S3d = Translation3d(2.,2.,2.)*Scaling(3.,2.,5.);    std::cout<<Translation3d(2.,2.,2.).vector()<<std::endl;    std::cout << "The vector v is of size " <<Translation3d(2.,2.,2.).vector().size()<< std::endl;    std::cout << "As a matrix, v is of size " << Translation3d(2.,2.,2.).vector().rows() << "x" << Translation3d(2.,2.,2.).vector().cols() << std::endl;//    2//    2//    2    std::cout<<Scaling(3.,2.,5.).toDenseMatrix ()<<std::endl;//    3 0 0//    0 2 0//    0 0 5    cout << S3d.matrix() << endl;//    3 0 0 2//    0 2 0 2//    0 0 5 2//    0 0 0 1    Affine3d eigenEE = Translation3d(2.,2.,2.)*m;    std::cout<<Translation3d(2.,2.,2.).vector()<<std::endl;    std::cout<<m<<std::endl;    cout << eigenEE.matrix() << endl;    Eigen::Matrix4d H = Eigen::Matrix4d::Identity();    H.block<3,3>(0,0) = m;    H.block<3,1>(0,3) << 2., 2., 2.;    cout << H.matrix() << endl;//Affine3d = Matrix4d, the last line is 0 0 0 1    cout << string(16,'=') << endl;    double droll = -0.186261;    double dpitch =  -0.437222;    double dyaw =  2.36416;    double dx = 0.475732;    double dy = 0.0143899;    double dz = 0.597381;    Eigen::Matrix3d R;    R = Eigen::AngleAxisd(droll, Eigen::Vector3d::UnitX()) *Eigen::AngleAxisd(dpitch, Eigen::Vector3d::UnitY())*Eigen::AngleAxisd(dyaw, Eigen::Vector3d::UnitZ()) ;    Eigen::Affine3d eigenCam;    eigenCam  = Eigen::Translation3d(dx, dy, dz)* R;    cout << eigenCam.matrix() << endl;//    -0.645673 -0.635467 -0.423424  0.475732//     0.633434 -0.755392  0.167766 0.0143899//    -0.426461  -0.15989  0.890262  0.597381//            0         0         0         1    Eigen::Vector4d r_tmp = eigenCam.matrix().col(3);// get last column, start from 0     cout << r_tmp << endl;//     0.475732//     0.0143899//     0.597381//     1    r_tmp[3] = 0;// the last element set to 0    cout << r_tmp << endl;//    0.475732//    0.0143899//    0.597381//    0    std::cerr << "L2Norm EE: "  << eigenCam.matrix().block(0,3,3,1) <<std::endl;// starting from (0,3),size 3x1    //     0.475732    //     0.0143899    //     0.597381    std::cerr << "L2Norm EE: "  << eigenCam.matrix().block(0,3,3,1).norm() <<std::endl;// starting from (0,3),size 3x1//    http://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html//    cout << eigenCam.matrix().block<2,2>(1,1) << endl << endl;// starting from (1,1), size 2x2//    cout << eigenCam.matrix().block(0,0,3,3) << endl << endl;// starting from(0,0), size 3x3}

2. 读取文件

一次读取一行 

while (ros::ok())一行行读取数据（读取指定行）

函数调用一次，下移一行

/home/yake/catkin_ws/src/handeye_calib_camodocal/src/handeye_calibration.cpp

#include<fstream>// Save to local file.#include <sstream> // stringstream, getlinestd::vector<std::string> split(const std::string &s, char delim){std::stringstream ss(s);std::string item;std::vector<std::string> elems;while (std::getline(ss, item, delim)){elems.push_back(item);}return elems;}

//    ===============================================================    string line;    ifstream infile1(kinova_filename);    int total_lines = std::count(std::istreambuf_iterator<char>(infile1), std::istreambuf_iterator<char>(), '\n');    cout << "Total lines = " << total_lines << endl;    infile1.close();    double droll ,dpitch ,dyaw ,dx ,dy ,dz;    droll = dpitch = dyaw = dx = dy = dz = 0.0;    Eigen::Affine3d eigenEE;    ifstream infile(kinova_filename);    if (infile.is_open())    {        cout << "Open file successfully." << endl;       static int curLine = 0;        Eigen::Matrix3d R;            if (curLine<total_lines)        {            infile.seekg(std::ios::beg);               for(int i=0; i < curLine; ++i)               {                   infile.ignore(std::numeric_limits<std::streamsize>::max(),'\n');               }            if( getline(infile, line))            {                ++curLine;                cout << "Linenum = " << curLine << endl;                std::vector<std::string> group_elems;                group_elems = (split(line, ','));                dx = atof(group_elems[0].c_str());                dy = atof(group_elems[1].c_str());                dz = atof(group_elems[2].c_str());                droll = atof(group_elems[3].c_str());                dpitch = atof(group_elems[4].c_str());                dyaw = atof(group_elems[5].c_str());                std::cerr<< dx <<" "<< dy <<" "<< dz <<" "<< droll <<" "<< dpitch<<" "<<dyaw<<endl;                R = Eigen::AngleAxisd(droll, Eigen::Vector3d::UnitX()) *Eigen::AngleAxisd(dpitch, Eigen::Vector3d::UnitY())*Eigen::AngleAxisd(dyaw, Eigen::Vector3d::UnitZ()) ;                eigenEE  = Eigen::Translation3d(dx, dy, dz)* R;                baseToTip.push_back(eigenEE);                cout << "Getting " << baseToTip.size ()<< " point of robot." << endl;            }        }        infile.close();    } else  cout << "Unable to open file" << endl;

一次读取整个文件

还有在一次while中读取完然后push到缓存队列中

D:\jaco\ConsoleApplication1\ConsoleApplication1\move2_multiable_elbow_fromFile.cpp

string line;ifstream infile1(rng_cmd_filename);int total_lines = std::count(std::istreambuf_iterator<char>(infile1), std::istreambuf_iterator<char>(), '\n');cout << "Total lines = " << total_lines << endl;infile1.close();// 【Base point】double cartesian[6] = { };ifstream infile(rng_cmd_filename);if (infile.is_open()){ cout << "Open file successfully." << endl; int curLine = 0; while (getline(infile, line))  {    curLine++;    cout << "Linenum = " << curLine << endl;    std::vector<std::string> group_elems;    group_elems = (split(line, ','));    // Current line 6 parameters.    for (int index = 0; index < 6; index++)   {    cartesian[index] = atof(group_elems[index].c_str());    }    for (int i = 0; i < 6; ++i)   {     cout << cartesian[i] << " ";   }    cout << endl;   TrajectoryPoint pointToSend = array2KinovaTrajPoint(cartesian);   cout << "Sending the " << curLine<< " point to the robot." << endl;   MySendAdvanceTrajectory(pointToSend);// Save to queue   Sleep(2000);   }    infile.close();} else  cout << "Unable to open file" << endl;

3. 保存文件

#define _USE_MATH_DEFINES // Use M_PI
#include <math.h>

#include<fstream>// Save to local file.

// To erase old data.ofstream output_file0("D:\\jaco\\Kinova_pose.txt", ios::out | ios::trunc);output_file0.close();ofstream output_file("D:\\jaco\\Kinova_pose.txt", ios::out | ios::app | ios::ate); ofstream output_file2("D:\\jaco\\Kinova_pose_all.txt", ios::out | ios::app | ios::ate);if (output_file.is_open() && output_file2.is_open()) {  cout << "Open file successfully." << endl;  output_file << RealcartesianX << "," << RealcartesianY << "," << RealcartesianZ << "," << RealcartesianThetaX << "," << RealcartesianThetaY << "," << RealcartesianThetaZ << endl;  output_file2  << RealcartesianX << "," << RealcartesianY << "," << RealcartesianZ << "," << RealcartesianThetaX << "," << RealcartesianThetaY << "," << RealcartesianThetaZ << endl;  output_file.close();  output_file2.close();  cout << "Writing down!" << endl; } else  cout << "Unable to open output file" << endl;cout << "*********************************" << endl;cout << "Cartesian Real(X Y Z RotX RotY RotZ) rad:" << RealcartesianX << " " << RealcartesianY << " " << RealcartesianZ << " " << RealcartesianThetaX << " " << RealcartesianThetaY << " " << RealcartesianThetaZ << endl;cout << "Cartesian Real(X Y Z RotX RotY RotZ) degree:" << RealcartesianX << " " << RealcartesianY << " " << RealcartesianZ << " " << RealcartesianThetaX * 180 / M_PI << " " << RealcartesianThetaY * 180 / M_PI << " " << RealcartesianThetaZ * 180 / M_PI << endl;