변환행렬 사원수 변환 코드(Matrix to Quaternion using Eigen Library)

3차원 4x4변환행렬을 알고 있을 때 

사원수의 w, x, y, z를 계산하는 것을 Eigen Library에서 제공하고 있어 코드를 소개합니다.

 

Quaternion은 우리말로 사원수라고 하는데 사원수 식은 아래와 같이 표현할 수 있습니다.
q = w+xi+yj+zk

 

우리가 잘 알고 있는 오일러 각을 사용하면 짐벌락(Gimbal Lock) 현상이 발생하는데 이 현상을 피하기 위해서는 사원수를 사용해야 합니다. 사원수를 사용하기 위해서는 위 식의 w, x, y, z를 알아야 합니다. 참고로 i, j, k는 허수 부분을 나타내는 세 개의 가상 백터입니다. 

4x4 변환행렬 입력으로 사원수의 w, x, y, z를 구하는 코드입니다.(행렬의 4행의 값은 translate에 관련된 값입니다.)

	Eigen::Matrix4d mat;
	mat << -0.997163, -0.068978, -0.030143, 0,
			0.071446, -0.993338, -0.090420, 0,
			-0.023705, -0.092317, 0.995447, 0,
			3.033070, 24.185200, 9.667200, 1;

	Eigen::Quaterniond quaternion(mat.topLeftCorner<3, 3>());
	float x = static_cast<float>(quaternion.x());
	float y = static_cast<float>(quaternion.y());
	float z = static_cast<float>(quaternion.z());
	float w = static_cast<float>(quaternion.w());

	// Quaternion 출력
	std::cout << "Quaternion: " << quaternion.coeffs().transpose() << std::endl;

 

반대로 사원수 w, x, y, z를 알고 있을 때 3x3 변환행렬을 구하는 코드입니다. 사원수는 회전에 관련된 수치만 계산되기 때문에 translate은 따로 계산해줘야 합니다.

	Eigen::Quaterniond quaternion_tomat(w,x,y,z);
	Eigen::Matrix3d rotation_matrix = quaternion_tomat.toRotationMatrix();

	// 회전 행렬 출력
	std::cout << "Rotation Matrix:\n" << rotation_matrix << std::endl;