Eigen 형변환 :Quaterniond, Vector3d to Matrix4d

Eigen을 활용해서 구현을 자주 하는데 그 때마다 quaternion과 translation을 표현하는 변수 타입이 섞이는 일이 많다. 

 

Vector4d가 편할 때, Quaterniond가 편할 때, Matrix4d가 편할 때가 각각 달라서 하나로 통일하긴 뭐하고 매번 서로 변환해가면서 쓴다.

 

같은 함순데 매번 새로 구현하는 것이 귀찮아서 이곳에 기록해둔다. 

 

그냥 값을 일일이 복붙하는 식으로 구현해도 되지만 뒤에 ceres solver라도 붙이려고 한다면 메모리 주소가 공유돼서 연결되어 있어야 하므로 다음과 같이 구현하는 것이 좋더라. 

void qt2mat(Eigen::Quaterniond q, Eigen::Vector3d t, Eigen::Matrix4d& mat){
    mat.block<3,3>(0,0) = q.toRotationMatrix();
    mat.block<3,1>(0,3) = t;
    mat(3, 3) = 1.0;
}

void mat2qt(Eigen::Matrix4d mat, Eigen::Quaterniond& q, Eigen::Vector3d& t){
    q = Eigen::Quaterniond(mat.block<3,3>(0,0));
    t = Eigen::Vector3d(mat.block<3,1>(0,3));
}