在博文<三維空間中剛體運動>中介紹了三維空間剛體運動和座標轉換的概念，包括座標變換的各中表示：旋轉矩陣、四元數、旋轉角、旋轉向量、變換矩陣等，初步嘗試了使用Eigen進行座標變換。本文在此基礎上，進一步總結Eigen中各種數據類型及其基本操作，便於查閱。
旋轉矩陣
構造函數
-
單位矩陣構造
Eigen::Matrix3d rotation_m=Eigen::Matrix3d::Identity(); -
由旋轉向量構造
rotation_m=rotation_v.toRotationMatrix();
常用函數
-
進行旋轉變換
Eigen::Vector3d p(1,0,0);//P點 Eigen::Vector3d p_r=m*p; cout<<p<<endl<<"after p rotated:"<<p_r<<endl<<endl;
旋轉向量
構造函數
-
旋轉軸和旋轉角構造. 比如旋轉軸向量爲[0,0,1],旋轉角爲M_PI/4時,初始化如下:
Eigen::AngleAxisd rotation_v(M_PI/4,Eigen::Vector3d(0,0,1)); -
由旋轉矩陣構造
常用函數
-
轉換爲Matrix格式
cout<<"rotation_v.matrix:\n"<<rotation_v.matrix()<<endl<<endl; -
使用進行旋轉
//使用旋轉向量進行旋轉變換 Eigen::Vector3d p(1,0,0);//P點 Eigen::Vector3d p_r=rotation_v*p; cout<<p<<endl<<"after p rotated:"<<p_r<<endl<<endl;
歐拉角
構造函數
-
由旋轉矩陣獲得
Eigen::Vector3d euler_a=rotation_m.eulerAngles(2,1,0); cout<<"yaw pitch roll="<<euler_a.transpose()<<endl<<endl;
四元數
構造函數
-
直接賦值,eg:
Quaterniond q(1.0,0.0,0.0,0.0);
賦值的順序是先實數,再虛數:w,x,y,z,而內部存儲則是先虛數,再實數:x,y,z,w -
從旋轉矩陣構造,eg:
Matrix3d mat; Quaterniond q(mat);
-
從旋轉向量構造
VectorXd vq(4); vq<<1.0, 0, 0, 0; Quaterniond qv(vq);
-
使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化四元數,
即使用當旋轉軸向量爲N=[n_x,n_y,n_z],旋轉角度爲A時,四元數初始化爲:
q=[cos(A/2),n_xsin(A/2),n_ysin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI/4)/2), 0*sin((M_PI/4)/2), 0*sin((M_PI/4)/2), 1*sin((M_PI/4)/2));//以（0,0,1）爲旋轉軸，旋轉45度
-
從數組構造
數組的順序應該是[w x y z]
常用函數
-
輸出係數
q.coeffs(); //[x y z w] -
輸出虛部
q.vec(); //[x y z]
以上兩種輸出都是以Eigen中的向量Vector形式輸出 -
輸出旋轉矩陣
要注意的是，只有單位四元數才表示旋轉矩陣，所以要先對四元數做單位化
q.normalized(); //important Matrix3d R=q.toRotationMatrix();
-
輸出單個元素
cout<<q.w()<<" "<<q.x()<<" "<<q.y()<<" "<<q.z()<<endl; -
共軛/即反向旋轉
一般不用inverse，在表示旋轉的時候（範數是1），共軛即可表示相反的的旋轉。
//q.inverse(); q.conjugate();
平移向量
構造
-
直接構造
Eigen::Vector3d t1 = Eigen::Vector3d(0.3, 0.1, 0.1);
變換矩陣
初始化
-
初始化爲單位矩陣
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity(); -
從旋轉矩陣和平移向量賦值
T1.rotate(q1.toRotationMatrix()); T1.pretranslate(t1);
常用函數
-
獲得Matrix:
cout << T1.matrix() << endl; -
求逆:
T1.inverse() -
變換運算
//歐拉矩陣求解 p2 = T2 * T1.inverse() * p1; cout << p2.transpose() << endl; //轉置