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转到旋转矩阵

Eigen: The Matrix class

#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;
//1 2 3
//4 5 6
//7 8 9
  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;
//    Eigen::Matrix4d 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

//    eigenCam  = R* Eigen::Translation3d(dx, dy, dz);
//    cout << eigenCam.matrix() << endl;

    eigenCam.rotation ();
//    Vector3f ea = a.eulerAngles(0, 1, 2);
//    cout<<"Euler angle: \n"<<eigenCam.rotation ().eulerAngles(0, 1, 2)<<"\n\n"<<endl;
    cout<<"Euler angle: \n"<<eigenCam.rotation ().eulerAngles(2, 1, 0)<<"\n\n"<<endl; // Z Y X
//    cout<<"Euler angle: \n"<<eigenCam.rotation ().rpyAngles()<<"\n\n"<<endl;


    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
}

eigen 旋转矩阵和四元数 

  double a_rad = pose.a* M_PI/180.0;
        double b_rad = pose.b* M_PI/180.0;
        double c_rad = pose.c* M_PI/180.0;
        printf("\n End pose(m, rad) %f,%f,%f,%lf,%lf,%lf\n", pose.x/1000.0, pose.y/1000.0, pose.z/1000.0, a_rad, b_rad, c_rad);


        // tool1 工具坐标系参数, 索引为1
        
        // baseHend 的姿态
            Eigen::Matrix3d R;
            R = Eigen::AngleAxisd(a_rad, Eigen::Vector3d::UnitX())
                    *Eigen::AngleAxisd(b_rad, Eigen::Vector3d::UnitY())
                    *Eigen::AngleAxisd(c_rad, Eigen::Vector3d::UnitZ()) ;

            // base_H_end 的齐次矩阵
            Matrix4d base_H_end = Eigen::Matrix4d::Identity();
            base_H_end.block<3,3>(0,0) = R;
            base_H_end.block<3,1>(0,3) << pose.x/1000.0, pose.y/1000.0, pose.z/1000.0;
            cout <<"baseHend: "<< base_H_end.matrix() << endl;

            // end_H_tool的姿态
            printf("\n###Now tool%d pose: %f, %f, %f, %f, %f, %f\n", tool_index, tool_pose.x, tool_pose.y, tool_pose.z, tool_pose.a, tool_pose.b, tool_pose.c);
            Eigen::Matrix3d R_endtool;

            double tool_pose_a_rad = tool_pose.a * M_PI/180.0;
            double tool_pose_b_rad = tool_pose.b * M_PI/180.0;
            double tool_pose_c_rad = tool_pose.c * M_PI/180.0;

            R_endtool = Eigen::AngleAxisd(tool_pose_a_rad, Eigen::Vector3d::UnitX())
                    *Eigen::AngleAxisd(tool_pose_b_rad, Eigen::Vector3d::UnitY())
                    *Eigen::AngleAxisd(tool_pose_c_rad, Eigen::Vector3d::UnitZ()) ;

            // end_H_tool的齐次矩阵
            Eigen::Matrix4d end_H_tool = Eigen::Matrix4d::Identity();
            end_H_tool.block<3,3>(0,0) = R_endtool;
            end_H_tool.block<3,1>(0,3) << tool_pose.x/1000.0, tool_pose.y/1000.0, tool_pose.z/1000.0;
            cout <<"end_H_tool: "<< end_H_tool.matrix() << endl;


            // base_H_tool的齐次矩阵
            Eigen::Matrix4d base_H_tool = base_H_end * end_H_tool;

            cout <<"base_H_tool: "<< base_H_tool.matrix() << endl;

            // base_H_tool 四元数
            Eigen::Quaterniond base_Q_tool(base_H_tool.block<3,3>(0,0));

            Eigen::Matrix3d rotation_matrix1(base_H_tool.block<3,3>(0,0));
            Eigen::Vector3d eulerAngle1 =rotation_matrix1.eulerAngles(0,1,2);
            cout << "eulerAngle1(deg), x y z: " << eulerAngle1*180/M_PI << endl;

            printf("==================\n\n");
            printf("%f,%f,%f,%lf,%lf,%lf,%lf\n", base_H_tool.block<3,1>(0,3)(0), base_H_tool.block<3,1>(0,3)(1), base_H_tool.block<3,1>(0,3)(2), base_Q_tool.x(), base_Q_tool.y(), base_Q_tool.z(), base_Q_tool.w());
            printf("==================\n\n");

Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换 (qq.com)

另参考

• http://blog.163.com/jiaqiang_wang/blog/static/11889615320147734750487/ Matlab与Eigen对应参考

#include <tf_conversions/tf_kdl.h>

MatrixXd initial_pose_quaternion(1,7); 
initial_pose_quaternion <<0.310199, 0.202714, -0.082002, -0.73323, 0.0342688, -0.0189734, 0.678851;

    Eigen::Quaterniond kinova_quaternion(initial_pose_quaternion(0,6), initial_pose_quaternion(0,3), initial_pose_quaternion(0,4), initial_pose_quaternion(0,5));
    MatrixXd kinova_pose_xyz(1,3);
    kinova_pose_xyz <<   initial_pose_quaternion(0,0),  initial_pose_quaternion(0,1), initial_pose_quaternion(0,2);

    Kinova_pose_homo= Eigen::Matrix4d::Identity();
    Kinova_pose_homo.block<3,3>(0,0) = kinova_quaternion.toRotationMatrix();
    Kinova_pose_homo.block<3,1>(0,3) =kinova_pose_xyz.transpose();

// X, Y, Z, W
    KDL::Frame t_transform(KDL::Rotation::Quaternion(initial_pose_quaternion(0,3), initial_pose_quaternion(0,4), initial_pose_quaternion(0,5),initial_pose_quaternion(0,6) ));
    KDL::Rotation M = out_transform.M;

    double roll, pitch, yaw;
    M.GetRPY(roll, pitch, yaw);
    cout<<"RPY=\n"<<roll<< " "<< pitch<<" "<<yaw<<endl;

    //    1.6 -0.2 1.1
    //    quaternion = tf.transformations.quaternion_from_euler(90*3.1415/180, 0, 0,'rxyz')
    srv.request.ThetaX=roll;
    srv.request.ThetaY=pitch;
    srv.request.ThetaZ=yaw;

    client.call(srv);

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, getline

std::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;
}

注意在windows下

#define NOMINMAX
#include <Windows.h>

//    ===============================================================
    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;

==================================

/home/yake/catkin_ws/src/iiwa_yake/src/iiwa_jv_cartesianV_jacobian.cpp

std::vector<double> ——> Eigen::VectorXd
Eigen::VectorXd qdot = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(joint_v.data(), joint_v.size());

===================

seekg()/seekp()与tellg()/tellp()的用法详解_Phoenix_FuliMa的博客-CSDN博客_seekg用法

   infile.seekg(20,ios::beg);
   streampos sp2=infile.tellg();
   cout<<"from file topoint:"<<endl<<sp2<<endl;

   cout<<infile.rdbuf();            // 读出文件内容

实战中遇到的C++流文件重置的一个大陷阱： 为什么ifstream的seekg函数无效？_涛歌依旧的博客-CSDN博客_ifstream 重置