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 
#include 
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<(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"<(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
}

 

 

另参考

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

#include 

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"<

 

2. 读取文件

一次读取一行 

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

函数调用一次，下移一行

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

 

#include// Save to local file.
#include  // stringstream, getline

std::vector split(const std::string &s, char delim)
{
	std::stringstream ss(s);
	std::string item;
	std::vector elems;
	
	while (std::getline(ss, item, delim))
	{
		elems.push_back(item);
	}

	return elems;
}

 

注意在windows下

#define NOMINMAX
#include

//    ===============================================================
    string line;
    ifstream infile1(kinova_filename);

    int total_lines = std::count(std::istreambuf_iterator(infile1), std::istreambuf_iterator(), '\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::max(),'\n');

               }

            if( getline(infile, line))
            {
                ++curLine;
                cout << "Linenum = " << curLine << endl;
                std::vector 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<<" "<

一次读取整个文件

还有在一次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(infile1), std::istreambuf_iterator(), '\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 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

#include// 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 ——> Eigen::VectorXd
Eigen::VectorXd qdot = Eigen::Map(joint_v.data(), joint_v.size());

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

https://blog.csdn.net/mafuli007/article/details/7314917

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

   cout<

https://blog.csdn.net/stpeace/article/details/40693951