로봇 SLAM의 위치변화 관계 및 공통 메시지 기록

소유하다

1. 회전 벡터

1.1 초기화
회전 각도: $a lp ha$ (시계 방향), 회전 축: $(엑스, y, z)$

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))

Eigen::AngleAxisd yawAngle(alpha,Vector3d::UnitZ());

1.2 회전 벡터 <-> 회전 행렬

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

1.3 회전 벡터 -> 오일러 각도

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);

1.4 회전 벡터를 쿼터니언으로 변환

Eigen::Quaterniond quaternion(rotation_vector);

Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

2. 회전 행렬

2.1 초기화

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

2.2 회전 행렬 -> 회전 벡터

Eigen::AngleAxisd rotation_vector(rotation_matrix);

Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

2.3 회전 행렬 -> 오일러 각도

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);

2.4 회전 행렬 -> 쿼터니언

Eigen::Quaterniond quaternion(rotation_matrix);

Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

3. 오일러 각도

3.1 초기화

Eigen::Vector3d eulerAngle(roll,pitch,yaw);

3.2 오일러 각도 -> 회전 벡터

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));

Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

3.3 오일러 각도->회전 행렬

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

3.4 오일러 각 -> 쿼터니언

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

4. 쿼터니언

4.1 초기화

Eigen::Quaterniond quaternion(w,x,y,z);

4.2 쿼터니언 -> 회전 벡터

Eigen::AngleAxisd rotation_vector(quaternion);

Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

4.3 쿼터니언 -> 회전 행렬

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

4.4 쿼터니언 -> 오일러 각도

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);

5. 아이소메트리3D

5.1 초기화

각 요소에 값을 할당

Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
T1(0,0) = 1.000000e+00, T1(0,1) = 1.197624e-11, T1(0,2) = 1.704639e-10, T1(0,3) = 3.214096e-14;
T1(1,0) = 1.197625e-11, T1(1,1) = 1.197625e-11, T1(1,2) = 3.562503e-10, T1(1,3) = -1.998401e-15;
T1(2,0) = 1.704639e-10, T1(2,1) = 3.562503e-10, T1(2,2) = 1.000000e+00, T1(2,3) = -4.041212e-14;
T1(3,0) =            0, T1(3,1) =            0, T1(3,2) =            0, T1(3,3) =             1;

회전 행렬 및 변환 벡터별

Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();
Tc1w.rotate(rotation_matrix); // 按照rotation_matrix进行旋转
Tc1w.pretranslate(t);   // 把平移向量设成t

Eigen::Matrix4d는 변환 행렬을 구성합니다.

Eigen::Matrix4d T2;
T2.setIdentity();
T2.block<3,3>(0,0) = rotation_matrix1;
T2.topRightCorner(3, 1) = t1;

5.2 회전 행렬 얻기

Eigen::Matrix3d rotation = Tc1w.rotation();

5.2 번역 벡터 얻기

Eigen::Vector3d position = Tc1w.translation();

6. Eigen::Affine3f와 Eigen::Matrix4f 간의 변환

Eigen::Affine3f A;
Eigen::Matrix4f M;
M = A.matrix();
A = M;

7. Float 및 Double 유형 변환

Eigen::MatrixXd matrix_d；
Eigen::MatrixXf matrix_f；
matrix_f = matrix_d.cast<float>();

nav_msgs::주행 거리 측정

Header header
	uint32 seq
	time stamp
	string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
	geometry_msgs/Point position
		float64 x
		float64 y
		float64 z
	geometry_msgs/Quaternion orientation
		float64 x
		float64 y
		float64 z
		float64 w
geometry_msgs/TwistWithCovariance twist
	geometry_msgs/Twist twist
		geometry_msgs/Vector3 linear
			float64 x
			float64 y
			float64 z
		geometry_msgs/Vector3 angular
			float64 x
			float64 y
			float64 z
	float64[36] covariance

지속적인 개선…

참고 기사:
고유 포즈 표현은
Eigen을 사용하여 쿼터니언, 오일러 각도, 회전 행렬 및 회전 벡터 간의 변환을 실현합니다.