Eigen Vector4d와 Quanterniond, 그리고 Ceres EigenQuaterniondParameterization()

Ceres를 이용하여 최적화 코드를 구현할 때 Eigen을 많이 활용한다. 특히 최적화 대상에 보통 rotation이 포함되는 경우가 많아서 Quaternion을 표기하는 과정에서 Eigen::Vector4d, Eigen::Quaterniond를 많이들 사용한다. 
 
하지만 주의할 점이 있어 기록해두고자 한다. 그 내용은 각각 어떤 convention을 따르느냐는 것이다. 다시 말해 (w,x,y,z) 순서인지, (x,y,z,w) 순서인지 말이다.
 

Eigen::Vector4d

Eigen::Vector4d는 (x,y,z,w) 순서를 사용한다. Hamilton convention으로 알려진 (w,x,y,z)이 아니다. 
 
argument 순서와 내부 데이터 저장 순서 동일하게 전부 (x,y,z,w) 순서를 사용한다. 
 
따라서 Eigen::Vector4d a(x,y,z,w) 와 같이 생성하면 되고 indexing도 a[0]이 x, a[3]이 w가 된다.
 

Eigen::Quaterniond

Eigen::Quaterniond가 끔찍한 놈이다. 
 
argument 순서로는 (w, x, y, z), hamilton convention을 따른다.
 
그러나 내부적으로는 (x, y, z, w) 순서로 저장된다.
 
따라서 Eigen::Quaterniond a(w, x, y, z) 와 같이 생성하되, indexing이나 메모리 주소로 접근할 경우 첫번째가 x, 마지막이 w다.
 

ceres::EigenQuaterniondParameterization()

ceres::Quaternion는 (w, x, y, z), hamilton convention을 따른다. argument 순서나 내부적으로나 동일하다. 
 
hamilton convention을 따르는 데이터라면 그대로 ceres::Quaternion으로 표현할 수 있다. 
 
(예를 들어, double array (wxyz 순서)를 ceres 변수로 만들고자 하면  ceres::QuaternionManifold 혹은 Ceres::QuaternionParameterization 사용하면 된다.)
 
하지만! ceres는 jacobian을 계산해야 하기 때문에 내부 메모리 주소에 접근하게 되는데 Eigen:Quaterniond는 겉과 속의 순서가 다른 놈이기 때문에 문제가 생긴다. 겉으로만 hamilton convention을 따르고 속으로는 (x, y, z, w)이기 때문이다.
 
따라서 Eigen::Quateriond 를 ceres 변수로 옮기려면 특별히 앞에 Eigen이 붙은 ceres 함수들을 사용해야 한다.
 
ceres::EigenQuaternionManifold 혹은 ceres::EigenQuaternionParameterization 사용해야 한다.
 
이것 안해주면 최적화 터진다.

  m_problem->AddParameterBlock(m_qs_per_time[time_id].data(), 4);  
  m_problem->SetParameterization(m_qs_per_time[time_id].data(), new ceres::EigenQuaternionParameterization());

 
아예 혼동 방지를 위해 .w() .x()와 같은 함수를 통해 구현하는 것이 좋고 아예 변환 함수를 구현해두는 것도 좋다.

    Eigen::Quaterniond vec2quat(Eigen::Vector4d qvec){
        Eigen::Quaterniond q(qvec.w(), qvec.x(), qvec.y(), qvec.z());
        return q;
    }