일을 하다가 4 DoF pose graph optimization (PGO)에 대한 얘기가 나와서, ‘오, VINS-Mono에서는 roll이랑 pitch는 고정한 채 4 DoF optimization하는 부분이 있다’라고 내가 코멘트를 해주었다.
하지만 원래 일이란 것이 아는 체 하면 내 일이 되지 않는가…(그렇게 나의 일이 되어 버렸다).
그래서 코드 레벨로 4 DoF PGO에 대해 자세히 살펴보았다.
원문 코드는 여기에서 확인할 수 있다.
코드 원문
void PoseGraph::optimize4DoF()
{
while(true)
{
int cur_index = -1;
int first_looped_index = -1;
m_optimize_buf.lock();
while(!optimize_buf.empty())
{
cur_index = optimize_buf.front();
first_looped_index = earliest_loop_index;
optimize_buf.pop();
}
m_optimize_buf.unlock();
if (cur_index != -1)
{
printf("optimize pose graph \n");
TicToc tmp_t;
m_keyframelist.lock();
KeyFrame* cur_kf = getKeyFrame(cur_index);
int max_length = cur_index + 1;
// w^t_i w^q_i
double t_array[max_length][3];
Quaterniond q_array[max_length];
double euler_array[max_length][3];
double sequence_array[max_length];
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
//options.minimizer_progress_to_stdout = true;
//options.max_solver_time_in_seconds = SOLVER_TIME * 3;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(0.1);
//loss_function = new ceres::CauchyLoss(1.0);
ceres::LocalParameterization* angle_local_parameterization =
AngleLocalParameterization::Create();
list<KeyFrame*>::iterator it;
int i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
(*it)->local_index = i;
Quaterniond tmp_q;
Matrix3d tmp_r;
Vector3d tmp_t;
(*it)->getVioPose(tmp_t, tmp_r);
tmp_q = tmp_r;
t_array[i][0] = tmp_t(0);
t_array[i][1] = tmp_t(1);
t_array[i][2] = tmp_t(2);
q_array[i] = tmp_q;
Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());
euler_array[i][0] = euler_angle.x();
euler_array[i][1] = euler_angle.y();
euler_array[i][2] = euler_angle.z();
sequence_array[i] = (*it)->sequence;
problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
if ((*it)->index == first_looped_index || (*it)->sequence == 0)
{
problem.SetParameterBlockConstant(euler_array[i]);
problem.SetParameterBlockConstant(t_array[i]);
}
//add edge
for (int j = 1; j < 5; j++)
{
if (i - j >= 0 && sequence_array[i] == sequence_array[i-j])
{
Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
relative_t = q_array[i-j].inverse() * relative_t;
double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, NULL, euler_array[i-j],
t_array[i-j],
euler_array[i],
t_array[i]);
}
}
//add loop edge
if((*it)->has_loop)
{
assert((*it)->loop_index >= first_looped_index);
int connected_index = getKeyFrame((*it)->loop_index)->local_index;
Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());
Vector3d relative_t;
relative_t = (*it)->getLoopRelativeT();
double relative_yaw = (*it)->getLoopRelativeYaw();
ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index],
t_array[connected_index],
euler_array[i],
t_array[i]);
}
if ((*it)->index == cur_index)
break;
i++;
}
m_keyframelist.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
//printf("pose optimization time: %f \n", tmp_t.toc());
/*
for (int j = 0 ; j < i; j++)
{
printf("optimize i: %d p: %f, %f, %f\n", j, t_array[j][0], t_array[j][1], t_array[j][2] );
}
*/
m_keyframelist.lock();
i = 0;
for (it = keyframelist.begin(); it != keyframelist.end(); it++)
{
if ((*it)->index < first_looped_index)
continue;
Quaterniond tmp_q;
tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));
Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);
Matrix3d tmp_r = tmp_q.toRotationMatrix();
(*it)-> updatePose(tmp_t, tmp_r);
if ((*it)->index == cur_index)
break;
i++;
}
Vector3d cur_t, vio_t;
Matrix3d cur_r, vio_r;
cur_kf->getPose(cur_t, cur_r);
cur_kf->getVioPose(vio_t, vio_r);
m_drift.lock();
yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();
r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));
t_drift = cur_t - r_drift * vio_t;
m_drift.unlock();
//cout << "t_drift " << t_drift.transpose() << endl;
//cout << "r_drift " << Utility::R2ypr(r_drift).transpose() << endl;
//cout << "yaw drift " << yaw_drift << endl;
it++;
for (; it != keyframelist.end(); it++)
{
Vector3d P;
Matrix3d R;
(*it)->getVioPose(P, R);
P = r_drift * P + t_drift;
R = r_drift * R;
(*it)->updatePose(P, R);
}
m_keyframelist.unlock();
updatePath();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}
Line-by-Line 설명
일단 cur_index가 valid하면, optimization이 시행된다.
그 전에 우리가 optimization하고 싶은 부분은 yaw 각도이기 때문에 rotation을 아래와 같은 R2ypr 함수를 통해 euler angle로 변환한다.
static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R)
{
Eigen::Vector3d n = R.col(0);
Eigen::Vector3d o = R.col(1);
Eigen::Vector3d a = R.col(2);
Eigen::Vector3d ypr(3);
double y = atan2(n(1), n(0));
double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
ypr(0) = y;
ypr(1) = p;
ypr(2) = r;
return ypr / M_PI * 180.0;
}
Eigen의 rpy 변환 코드를 쓰면 값이 부정확해지는 것을 경험적으로 알고 있는데, 아마 위의 코드는 그런 문제가 없지 않을까 싶다 (확인이 필요하지만, 잘 동작하는 코드이기 때문에 아마 맞을듯).
그 후, 4 DoF optimization을 위한 cost function을 만들어서 residual block을 추가한다.
해당 edge가 loop로 기인한 것이든 연속적인 frame 사이의 edge이든, 아래와 같은 FourDOFError 클래스를 통해 cost function을 만들어서 residual block을 추가한다.
Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());
Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
relative_t = q_array[i-j].inverse() * relative_t;
double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),
relative_yaw, euler_conncected.y(), euler_conncected.z());
problem.AddResidualBlock(cost_function, NULL, euler_array[i-j],
t_array[i-j],
euler_array[i],
t_array[i]);
여기서 relative translation과 rotation(yaw 각도)를 구하는 것을 확인할 수 있다.
먼저 아래의 수식으로 relative_t를 구한다:
구현 레벨에서 살펴보면, 처음 Vector3d relative_t를 선언한 부분은 t_2 - t_1를 계산하는 부분을 나타내고, q_array[i-j].inverse()가 위의
수식에서 transposed(inversed) rotation을 곱해주는 행위와 대응된다.
두번 째로는 relative_yaw로 direct하게 yaw angle의 차이를 빼서 계산하는 것을 볼 수 있다 (뭔가 엄청난 처리를 해줄 줄 알았는데, 단순히 yaw 각도를 뺀 값만 넣어줘서 별거 없구나 싶었다).
FourDOFError는 아래와 같이 선언되어 있다:
struct FourDOFError
{
FourDOFError(double t_x, double t_y, double t_z, double relative_yaw, double pitch_i, double roll_i)
:t_x(t_x), t_y(t_y), t_z(t_z), relative_yaw(relative_yaw), pitch_i(pitch_i), roll_i(roll_i){}
template <typename T>
bool operator()(const T* const yaw_i, const T* ti, const T* yaw_j, const T* tj, T* residuals) const
{
T t_w_ij[3];
t_w_ij[0] = tj[0] - ti[0];
t_w_ij[1] = tj[1] - ti[1];
t_w_ij[2] = tj[2] - ti[2];
// euler to rotation
T w_R_i[9];
YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
// rotation transpose
T i_R_w[9];
RotationMatrixTranspose(w_R_i, i_R_w);
// rotation matrix rotate point
T t_i_ij[3];
RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);
residuals[0] = (t_i_ij[0] - T(t_x));
residuals[1] = (t_i_ij[1] - T(t_y));
residuals[2] = (t_i_ij[2] - T(t_z));
residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relative_yaw));
return true;
}
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
const double relative_yaw, const double pitch_i, const double roll_i)
{
return (new ceres::AutoDiffCostFunction<
FourDOFError, 4, 1, 3, 1, 3>(
new FourDOFError(t_x, t_y, t_z, relative_yaw, pitch_i, roll_i)));
}
double t_x, t_y, t_z;
double relative_yaw, pitch_i, roll_i;
};
위의 코드를 보면 Create 함수에서 euler_conncected.y(), euler_conncected.z()를 입력으로 받는 것을 볼 수 있다.
여기서 주의할 건, euler_conncected.y(), euler_conncected.z()는 단순히 euler_conncected의 2번째(pitch), 3번째 요소(roll)를 의미한다.
즉, y축, z축의 회전에 대한 각도가 아님을 주의하자.
위에서 Ceres solver에서 4, 1, 3, 1, 3의 의미는
- Residual block 갯수 4
yaw_i의 차원 1ti의 차원 3yaw_j의 차원 1tj의 차원 3 이란 뜻이다.
그리고 operator() 내부에서 차이를 계산하는 과정은:
T t_w_ij[3];
t_w_ij[0] = tj[0] - ti[0];
t_w_ij[1] = tj[1] - ti[1];
t_w_ij[2] = tj[2] - ti[2];
// euler to rotation
T w_R_i[9];
YawPitchRollToRotationMatrix(yaw_i[0], T(pitch_i), T(roll_i), w_R_i);
// rotation transpose
T i_R_w[9];
RotationMatrixTranspose(w_R_i, i_R_w);
// rotation matrix rotate point
T t_i_ij[3];
RotationMatrixRotatePoint(i_R_w, t_w_ij, t_i_ij);
이 FourDOFError 밖에서 계산한 아래 부분이 정확히 동일하다:
Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);
relative_t = q_array[i-j].inverse() * relative_t;
double relative_yaw = euler_array[i][0] - euler_array[i-j][0];
하지만, loop edge가 추가되면서 loop closing 과정에서 관측된 measurement 값과 trajectory 상의 상대 값에 큰 차이가 발생한다.
그로 인해 FourDOFWeightError를 통해 trajectory가 수정되고, 위의 add edge 부분의 factor는 optimization 과정 중에 그래도 기존의 relative pose의 경향성을 유지해주게끔 도와준다.
결론
코드 내부에서 엘레강스(?)하게 4DoF를 다룰 줄 알았는데, 단순히 translation과 rotation을 decouple해서 optimization을 하는 것이 다였다. 이제는 GTSAM으로 이부분을 옮길 수 있을듯! (Luca 교수님께서 GTSAM의 main contributer이기 때문에 해당 연구실에서는 GTSAM을 주로 쓴다)