概述
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)<<"nn"<<endl;
cout<<"Euler angle: n"<<eigenCam.rotation ().eulerAngles(2, 1, 0)<<"nn"<<endl; // Z Y X
// cout<<"Euler angle: n"<<eigenCam.rotation ().rpyAngles()<<"nn"<<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,%lfn", 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, %fn", 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("==================nn");
printf("%f,%f,%f,%lf,%lf,%lf,%lfn", 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("==================nn");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:jacoConsoleApplication1ConsoleApplication1move2_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 重置
最后
以上就是甜蜜西牛为你收集整理的Eigen demo与文件读写 汇总的全部内容，希望文章能够帮你解决Eigen demo与文件读写 汇总所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复