Eigen的使用總結2——geometry

常用：

旋轉矩陣（3X3）:Eigen::Matrix3d
旋轉向量（3X1）:Eigen::AngleAxisd
四元數（4X1）:Eigen::Quaterniond
平移向量（3X1）:Eigen::Vector3d
變換矩陣（4X4）:Eigen::Isometry3d

AngleAxis（angle, axis）：繞該軸逆時針旋轉angle(rad)。

變換矩陣
Eigen::Isometry3d T;
T.matrix()纔是變換矩陣，做運算時需加.matrix()後綴;
T.pretranslate()以及T.prerotate()可以給平移部分和旋轉矩陣賦值，但是若循環中使用，末尾不重置變換矩陣的話，這個設置量會累加，而不是覆蓋。

四元數賦值：Eigen::Quaterniond Q;
Q.x() = 3 「類似地 Q.y() = Q.z() = Q.w()」。

旋轉矩陣（R），旋轉向量（V）和四元數（Q）在Eigen中轉換關係的總結：

2.旋轉矩陣（R），旋轉向量（V）和四元數（Q）分別通過自身初始化自己的方式：

R通過自身初始化的方法：
//1.使用旋轉矩陣的函數來初始化旋轉矩陣
Matrix3d R1=Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;

V通過自身初始化的方法：
//1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化角軸
AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）爲旋轉軸，旋轉45度

cout << "Rotation_vector1" << endl << V1.matrix() << endl;

Q通過自身初始化的方法：
//1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化四元數,即使用

q=[cos(A/2),n_x*sin(A/2),n_y*sin(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度

cout << "Quaternion1" << endl << Q1.coeffs() << endl;

演示代碼1：eigen_geometry.cpp

#include <iostream>
#include <Eigen/Dense>
#define M_PI 3.1415926

using namespace std;
using namespace Eigen;

void run_eigen_geometry()
{

	//下面三個變量作爲下面演示的中間變量

	AngleAxisd t_V(M_PI / 4, Vector3d(0, 0, 1));
	Matrix3d t_R = t_V.matrix();
	Quaterniond t_Q(t_V);


	//對旋轉向量（軸角）賦值的三類方法

	cout << "對旋轉向量（軸角）賦值的三類方法" << endl;
	//1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化角軸
	AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）爲旋轉軸，旋轉45度
	cout << "Rotation_vector1" << endl << V1.matrix() << endl;

	//2.通過旋轉矩陣的方式

	//2.1 使用旋轉向量的fromRotationMatrix()函數來對旋轉向量賦值（注意此方法爲旋轉向量獨有,四元數沒有）
	AngleAxisd V2;
	V2.fromRotationMatrix(t_R);
	cout << "Rotation_vector2" << endl << V2.matrix() << endl;

	//2.2 直接使用旋轉矩陣來對旋轉向量賦值
	AngleAxisd V3;
	V3 = t_R;
	cout << "Rotation_vector3" << endl << V3.matrix() << endl;

	//2.3 使用旋轉矩陣來對旋轉向量進行初始化
	AngleAxisd V4(t_R);
	cout << "Rotation_vector4" << endl << V4.matrix() << endl;

	//3. 使用四元數來對旋轉向量進行賦值

	//3.1 直接使用四元數來對旋轉向量賦值
	AngleAxisd V5;
	V5 = t_Q;
	cout << "Rotation_vector5" << endl << V5.matrix() << endl;

	//3.2 使用四元數來對旋轉向量進行初始化
	AngleAxisd V6(t_Q);
	cout << "Rotation_vector6" << endl << V6.matrix() << endl;


	//------------------------------------------------------

	//對四元數賦值的三類方法（注意Eigen庫中的四元數前三維是虛部,最後一維是實部）

	cout << "對四元數賦值的三類方法" << endl;
	//1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化四元數,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(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度
	//第一種輸出四元數的方式
	cout << "Quaternion1" << endl << Q1.coeffs() << endl;

	//第二種輸出四元數的方式
	cout << Q1.x() << endl << endl;
	cout << Q1.y() << endl << endl;
	cout << Q1.z() << endl << endl;
	cout << Q1.w() << endl << endl;

	//2. 使用旋轉矩陣轉四元數的方式

	//2.1 直接使用旋轉矩陣來對旋轉向量賦值
	Quaterniond Q2;
	Q2 = t_R;
	cout << "Quaternion2" << endl << Q2.coeffs() << endl;


	//2.2 使用旋轉矩陣來對四元數進行初始化
	Quaterniond Q3(t_R);
	cout << "Quaternion3" << endl << Q3.coeffs() << endl;

	//3. 使用旋轉向量對四元數來進行賦值

	//3.1 直接使用旋轉向量對四元數來賦值
	Quaterniond Q4;
	Q4 = t_V;
	cout << "Quaternion4" << endl << Q4.coeffs() << endl;

	//3.2 使用旋轉向量來對四元數進行初始化
	Quaterniond Q5(t_V);
	cout << "Quaternion5" << endl << Q5.coeffs() << endl;

	//----------------------------------------------------

	//對旋轉矩陣賦值的三類方法
	cout << "對旋轉矩陣賦值的三類方法" << endl;
	//1.使用旋轉矩陣的函數來初始化旋轉矩陣
	Matrix3d R1 = Matrix3d::Identity();
	cout << "Rotation_matrix1" << endl << R1 << endl;

	//2. 使用旋轉向量轉旋轉矩陣來對旋轉矩陣賦值

	//2.1 使用旋轉向量的成員函數matrix()來對旋轉矩陣賦值
	Matrix3d R2;
	R2 = t_V.matrix();
	cout << "Rotation_matrix2" << endl << R2 << endl;

	//2.2 使用旋轉向量的成員函數toRotationMatrix()來對旋轉矩陣賦值
	Matrix3d R3;
	R3 = t_V.toRotationMatrix();
	cout << "Rotation_matrix3" << endl << R3 << endl;

	//3. 使用四元數轉旋轉矩陣來對旋轉矩陣賦值

	//3.1 使用四元數的成員函數matrix()來對旋轉矩陣賦值
	Matrix3d R4;
	R4 = t_Q.matrix();
	cout << "Rotation_matrix4" << endl << R4 << endl;

	//3.2 使用四元數的成員函數toRotationMatrix()來對旋轉矩陣賦值
	Matrix3d R5;
	R5 = t_Q.toRotationMatrix();
	cout << "Rotation_matrix5" << endl << R5 << endl;

}

演示代碼2：eigen_geometry_2.cpp

#include <iostream>
#include <cmath>
#define M_PI 3.1415926
using namespace std;

#include <Eigen/Core>
// Eigen 幾何模塊
#include <Eigen/Geometry>

/****************************
* 本程序演示了 Eigen 幾何模塊的使用方法
****************************/

void run_eigen_geometry_2()
{
	//注意一下類型名的最後一個字符爲d表示雙精度類型,換成f表示單精度類型，兩種類型不能混用，必須顯示轉換
	// Eigen/Geometry 模塊提供了各種旋轉和平移的表示
	// 3D 旋轉矩陣直接使用 Matrix3d 或 Matrix3f
	/****旋轉向量****/

	// 旋轉向量使用 AngleAxis, 它底層不直接是Matrix，但運算可以當作矩陣（因爲重載了運算符）
	// 乘以該向量，表示進行一個座標變換
	//任意旋轉可用一個旋轉軸和一個旋轉角度來表示。
	//旋轉向量，旋轉向量的方向與旋轉軸一致，長度爲旋轉角度。
	
	/*********************************/
	/*旋轉向量　沿 Z 軸旋轉 45 度         角度　軸 */
	Eigen::AngleAxisd rotation_vector(M_PI / 4, Eigen::Vector3d(0, 0, 1));   //沿 Z 軸旋轉 45 度
	cout.precision(3);
	cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;     //用matrix()轉換成矩陣,也可以直接賦值
	
	/*********************************/
	/*旋轉矩陣*/
	Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//單位陣
	rotation_matrix = rotation_vector.toRotationMatrix();//轉成旋轉矩陣　由羅德里格公式進行轉換
	
	// 用 AngleAxis 可以進行座標變換
	Eigen::Vector3d v(1, 0, 0);
	/*************旋轉向量進行座標變換********************/
	Eigen::Vector3d v_rotated = rotation_vector * v;
	cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
	// 或者用旋轉矩陣
	/*****************旋轉矩陣進行座標變換****************/
	v_rotated = rotation_matrix * v;
	cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

	/**歐拉角表示的旋轉**/
	// 歐拉角: 可以將旋轉矩陣直接轉換成歐拉角
	Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX順序，即roll pitch yaw順序
	cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

	/***歐式變換矩陣表示旋轉**/
	// 歐氏變換矩陣使用 Eigen::Isometry
	Eigen::Isometry3d T = Eigen::Isometry3d::Identity();// 雖然稱爲3d，實質上是4＊4的矩陣　　齊次座標
	T.rotate(rotation_vector);                                        // 按照rotation_vector進行旋轉
	T.pretranslate(Eigen::Vector3d(1, 3, 4));               // 把平移向量設成(1,3,4)
	cout << "Transform matrix = \n" << T.matrix() << endl;
	// 用變換矩陣進行座標變換
	Eigen::Vector3d v_transformed = T*v;                              // 相當於R*v+t
	cout << "(1,0,0) after Isometry3d tranformed = " << v_transformed.transpose() << endl;


	// 對於仿射和射影變換，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略


	/*******四元數表示的旋轉***********/
	// 可以直接把AngleAxis賦值給四元數，反之亦然 Quaterniond 表示雙精度　四元素　Quaternionf　表示單精度四元素
	Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);// 表示沿Z 軸旋轉 45 度　的四元素變換 
	cout << "quaternion from AngleAxis rotation_vector = \n" << q.coeffs() << endl;   // 請注意coeffs的順序是(x,y,z,w),w爲實部，前三者爲虛部
	// 也可以把旋轉矩陣賦給它
	q = Eigen::Quaterniond(rotation_matrix);
	cout << "quaternion from rotation_matrix = \n" << q.coeffs() << endl;
	// 使用四元數旋轉一個向量，使用重載的乘法即可
	/*注意程序表達形式和實際運算的不一樣*/
	v_rotated = q*v; // 注意數學上是q*v*q^{-1}  而程序爲了簡化表示　直接使用　q*v代替
	cout << "(1,0,0) after Quaterniond rotation = " << v_rotated.transpose() << endl;
	
	
	
	/*編程題目
	小蘿蔔１號位姿q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]'　　　世界座標系到相機變換
	小蘿蔔２號位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-0.1,0.5,0.3]'
	小蘿蔔１號看到位於自身座標系下p=[0.5,0,0.2]'
	求該向量在小蘿蔔２號下的座標
	*/
	Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1);//wxyz q1.coeffs()  xyzw  q1.vec()  xyz
	//q1 << 0.35,0.2,0.3,0.1;
	Eigen::Matrix<double, 3, 1> t1;//float類型
	t1 << 0.3, 0.1, 0.1;
	Eigen::Quaterniond q2(-0.5, 0.4, -0.1, 0.2);
	//q2 << -0.5,0.4,-0.1,0.2;
	Eigen::Matrix<double, 3, 1> t2;//float類型
	t2 << -0.1, 0.5, 0.3;
	Eigen::Matrix<double, 3, 1> p1;//float類型
	p1 << 0.5, 0, 0.2;

	cout << "q1= \n" << q1.coeffs() << endl;
	cout << "t1= \n" << t1 << endl;
	cout << "q2= \n" << q2.coeffs() << endl;
	cout << "t2= \n" << t2 << endl;

	/*
	q1.setIdentity();
	cout<<"q1 after setIdentity \n"<<q1.coeffs() <<endl;
	q2.setIdentity();
	cout<<"q2 after setIdentity \n"<<q2.coeffs() <<endl;
	*/
	
	q1 = q1.normalized();//規範化　　歸一化   除以模長
	cout << "q1 after normalized\n" << q1.coeffs() << endl;
	q2 = q2.normalized();
	cout << "q2 after normalized \n" << q2.coeffs() << endl;

	Eigen::Matrix3d q1rotation_matrix = Eigen::Matrix3d::Identity();//單位陣
	q1rotation_matrix = q1.toRotationMatrix();
	Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();// 雖然稱爲3d，實質上是4＊4的矩陣　　齊次座標

	Tc1w.rotate(q1rotation_matrix);                                    // 按照q1rotation_matrix進行旋轉
	Tc1w.pretranslate(t1);                                                     // 把平移向量設成t1

	//Eigen::Isometry3d Twc1=Tc1w.inverse();//由world 到c1的逆變換　　成　c1到world
	Eigen::Matrix<double, 3, 1> pw = Tc1w.inverse()*p1;    //將c1座標系下的點p1變換到world座標系下

	Eigen::Matrix3d q2rotation_matrix = Eigen::Matrix3d::Identity();//單位陣
	q2rotation_matrix = q2.toRotationMatrix();
	Eigen::Isometry3d Tc2w = Eigen::Isometry3d::Identity();// 雖然稱爲3d，實質上是4＊4的矩陣　　齊次座標

	Tc2w.rotate(q2rotation_matrix);                                    // 按照q1rotation_matrix進行旋轉
	Tc2w.pretranslate(t2);                                                     // 把平移向量設成t1

	Eigen::Matrix<double, 3, 1> p2 = Tc2w*pw;    //將world座標系下的點pw變換到c2座標系下
	cout << "the loc of p1 in c1  = \n" << p1 << endl;
	cout << "the loc of p1 in world  = \n" << pw << endl;
	cout << "the loc of p1 in c2 = \n" << p2 << endl;

}

參考：https://blog.csdn.net/u011092188/article/details/77430988

Eigen的使用總結2——geometry

MySQL 核心模塊揭祕 | 18 期 | 鎖在內存里長什麼樣*

使用perf工具生成火焰圖

HttpSecurity 是如何組裝過濾器鏈的

數說海南——近6年海南各市縣人口簡單看

長序列中Transformers的高級注意力機制總結

響應式界面控件DevExtreme * 更強的數據分析和可視化功能

指針定義後，未初始化指向，運行時會報錯：段錯誤 (核心已轉儲)

Boost.mutex連續兩次加鎖會造成死鎖

C++11併發與多線程總結

C/C++指針作爲函數形參注意點

wireshark 進行網絡抓包

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結