한 줄 요약
겁나 복잡하고 어렵다
Introduction
지난 포스트에서는 DSO의 Photometric Energy Term, 즉 Residual을 구하는 이론적 접근을 다뤘습니다. 이번 포스트에서는 이론을 실전으로 옮겨보도록 하겠습니다. 과연 어떻게 코드에서는 구현되어 있을까요?
일단 어디에 구현되어 있는지부터 봅시다. 일단 해당 내용은 CoarseInitializer.cc의 calcResAndGS() 함수에 구현되어 있습니다. 이는 DSO 초기화를 담당하는 구현부인데요. 초기화 영역 말고 실행 중에도 Residual은 계속 호출이 될 것입니다. 이는 CoarseTracker.cc의 calcRes() 함수에 구현되어 있으나, 이번 포스팅에서는 초기화 부분만 우선 살펴보도록 하겠습니다.
변수 초기화
우선 너무나 당연하게도 변수를 초기화해 줍니다. 여기서 초기화해 주는 변수들은 Residual을 구하기 위한 두 프레임과 관련되어 있습니다. 필요한 변수들이 무엇인지는 이전 포스트의 Notation 부분을 참고하면 알 수 있습니다. 아래는 해당 부분의 코드이며, 이중 RKi와 t는 꼭 기억해두는게 좋습니다. 각각 $\mathbf{RK^{-1}}$와 $\mathbf{t}$를 저장하는 변수이기 때문에 계산 과정에 꼭 등장할 예정입니다.
// CoarseInitializer::calcResAndGS()
// load width and height of image pyramid
int wl = w[lvl], hl = h[lvl];
// load Image of first and current frames
Eigen::Vector3f *colorRef = firstFrame->dIp[lvl];
Eigen::Vector3f *colorNew = newFrame->dIp[lvl];
// multiplex of rotation matrix and inverse matrix of intrinsic camera
// parameter
Mat33f RKi = (refToNew.rotationMatrix() * Ki[lvl]).cast<float>();
// translation matrix (first image -> current image)
Vec3f t = refToNew.translation().cast<float>();
// generate affine brightness transfer parameters
Eigen::Vector2f r2new_aff =
Eigen::Vector2f(exp(refToNew_aff.a), refToNew_aff.b);
// get intrinsic parameters of image pyramid
float fxl = fx[lvl];
float fyl = fy[lvl];
float cxl = cx[lvl];
float cyl = cy[lvl];
$\mathbf{p}'$ 구하기
이제 초기화를 끝냈으면, 본격적으로 Residual을 구해봅시다. 점 $\mathbf{p}$에 대한 Residual을 구하기 위해서는 Residual pattern을 고려해 총 8개의 점에 대한 Energy를 계산해야 합니다.
이전 포스트에서 점 $\mathbf{p}'$를 구하는 법을 유도했는데요. 그 과정에서 아래의 식이 유도되었습니다. 이를 조금만 변형하면 우리가 원하는 형태를 얻을 수 있으니 같이 따라가 봅시다.
$$\mathbf{P}_{mn}=d_{\mathbf{P}}\mathbf{K}(\mathbf{RK}^{-1}\mathbf{P}_{uv}/d_{\mathbf{P}}+\mathbf{t})$$
$$=\mathbf{K}(\mathbf{RK}^{-1}\mathbf{P}_{uv}+d_{\mathbf{p}}\mathbf{t})$$
여기서 $(m, n)$은 현재 Frame에 투영된 벡터 $\mathbf{P}$의 좌표이며, 반대로 $(u, v)$는 첫 번째 Frame에서의 좌표입니다. 코드와 변수명을 일치시키기 위해 이전 포스트와는 표기법을 살짝 다르게 했으니 양해 바랍니다. 위 식에서 Intrinsic parameter $\mathbf{K}$를 제외한 $(\cdot)$ 내부를 잘 기억하세요. 코드에서는 해당 부분으로 전개를 하기 시작합니다.
사실 이 부분에서 이상한 점이 하나 있는데요. 일단 다 살펴본 뒤 마지막에 해당 내용을 살펴보도록 하겠습니다.
// Reference: CoarseInitializer::calcResAndGS()
// iterate for residual pattern, patternNum is 8
for (int idx = 0; idx < patternNum; idx++) {
int dx = patternP[idx][0];
int dy = patternP[idx][1];
// calculate inner part of p' calculation, except multiplexing K
Vec3f pt =
RKi * Vec3f(point->u + dx, point->v + dy, 1) + t * point->idepth_new;
// normalize Z axis, to normalized plane (z=1)
float u = pt[0] / pt[2];
float v = pt[1] / pt[2];
// calculate new (u, v)
float Ku = fxl * u + cxl;
float Kv = fyl * v + cyl;
// update inverse depth
float new_idepth = point->idepth_new / pt[2];
if (!(Ku > 1 && Kv > 1 && Ku < wl - 2 && Kv < hl - 2 && new_idepth > 0)) {
isGood = false;
break;
}
여기서 patternP는 앞서 소개해드린 Residual Pattern입니다. SSE2 레지스터 사용을 위해 과감히 bottom-right을 제외시킨 과감한 패턴이었죠. 기억이 가물가물하다면 이전 포스트를 참고 부탁 드립니다. 아무튼 해당 패턴의 정보는 Settings.h/Setting.cc를 통해 확인할 수 있습니다. 왜 이름이 다른 거지...
// Reference: Settings.h
// use the ninth pattern (described in DSO's paper)
#define patternP staticPattern[8]
// Reference: Setting.cc
// 10 kinds of patterns
// -100 means "not used"
int staticPattern[10][40][2] = {
...
// the pattern used in dso's paper
/**
* |_|_|_|#|_|_|_|
* |_|_|#|_|#|_|_|
* |_|#|_|#|_|#|_|
* |_|_|#|_|_|_|_|
* |_|_|_|#|_|_|_|
*/
{{0, -2}, {-1, -1}, {1, -1}, {-2, 0}, {0, 0}, {2, 0}, {-1, 1}, {0, 2}, {-100, -100}, {-100, -100}, // 8 for SSE efficiency
{-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
{-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
{-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}},
...
};
Residual Modeling
이제 앞서 구한 정보들을 토대로 Residual을 구해봅시다. 이전 포스트에서 다룬 식에 대한 모델링을 다시 한번 보시면 좋을 것 같습니다. 다만 논문과 제가 정리한 글과도 다른 부분은 Huber Norm을 그대로 적용하지 않고, 좀 더 compact 하게 적용했다는 점입니다. 사실 이렇게 보니 직관적으로 이해하기가 쉽지 않기 때문에, 보편적인 표기법으로 Huber Norm을 적용했으면 어떨까 하는 마음이 있습니다. 간소화된 Huber weight와 Huber norm은 아래와 같습니다. $r$은 계산된 Residual을 의미하고, $w_h$는 Huber weight를 나타냅니다.
$$w_h=\left\{\begin{matrix}
1 & |r|<\alpha \\
\frac{|r|}{\alpha} & |r|\geq \alpha \\
\end{matrix}\right.$$
$$H(r)=w_hr^2(1-\frac{w_h}{2})$$
// Reference: CoarseInitializer::calcResAndGS()
// get intensity of p from first frame, p' from current frame
Vec3f hitColor = getInterpolatedElement33(colorNew, Ku, Kv, wl);
float rlR = getInterpolatedElement31(colorRef, point->u + dx, point->v + dy, wl);
// check the intensity whether it is acceptable or not
if (!std::isfinite(rlR) || !std::isfinite((float)hitColor[0])) {
isGood = false;
break;
}
// Photometric Energy Term Modeling
float residual = hitColor[0] - r2new_aff[0] * rlR - r2new_aff[1];
// Add Huber norm with compact huber weight
float hw = fabs(residual) < setting_huberTH
? 1
: setting_huberTH / fabs(residual);
// add energy H(r)*2
energy += hw * residual * residual * (2 - hw);
하나 특이한 점은 계산의 편의성인지 모르겠지만, 위에서 유도한 $H(r)$에 2를 곱해서 최종 Photometric energy인 energy에 더해주고 있습니다.
Question
앞서 코드에 이상한 점이 하나 있다고 했는데요. 혹시 기억하지 못하시면 $\mathbf{p}'$ 구하기 챕터를 다시 한번 봐주세요. 아래 수식과 코드에서 이상한 점이 있는데요.
아래 수식의 $\mathbf{P}_{mn}, \mathbf{P}_{uv}$는 모두 Pixel coordinate 상의 좌표입니다. 즉, 이미 Z 좌표가 normalization을 거친 후입니다. 하지만 코드에서는 $\mathbf{P}_{mn}$를 구한 후 Z 축 좌표에 해당하는 pt[2]로 pt[0], pt[1]을 나누어 u와 v를 구합니다. 즉, inverse depth를 두 번 곱하게 됩니다.
$$\mathbf{P}_{mn}=\mathbf{K}(\mathbf{RK}^{-1}\mathbf{P}_{uv}+d_{\mathbf{p}}\mathbf{t})$$
// Reference: CoarseInitializer::calcResAndGS()
// calculate inner part of p' calculation, except multiplexing K
Vec3f pt =
RKi * Vec3f(point->u + dx, point->v + dy, 1) + t * point->idepth_new;
// normalize Z axis, to normalized plane (z=1)
float u = pt[0] / pt[2];
float v = pt[1] / pt[2];
이런 계산식 설계 오류가 최종 결과에 방해가 되지 않을까요? 놀랍게도 답은 '아니다'입니다. 왜 아닐까요? 우리가 $\mathbf{P}_{mn}$의 좌표를 구하는 순간부터 이미 Z 좌표는 1로 고정되어 있었습니다. 즉, 최종적으로 u, v를 구하기 위해 나누는 행위는 모두 1로 나누고 있다는 의미입니다. 아무런 의미 없는 행위이죠. 해당 부분을 코드에서 제거한다면, 나머지 연산에 대한 오버헤드가 줄어 속도 향상에 도움이 될지도 모르겠네요. 1로 나누는 것뿐이라 도움이 될지는 잘 모르겠습니다. 아래는 해당 pt[2]들을 출력한 결과물입니다. 전부 1을 나타내고 있죠.
Conclusion
이번 포스트에서는 DSO/LDSO에서 어떻게 Photometric Energy Term, Residual을 구현했는지 살펴봤습니다. 여러 트릭이 들어가 있다고 생각되었는데요. 개인적으로 꽤나 해석이 어려운 불친절한 코드 중 하나라고 생각합니다. 그래도 수식을 차근차근 따라가며 최적화 가능 포인트도 발굴했으니 꽤나 열심히 봤다고 생각합니다. 하지만 여전히 inverse depth를 업데이트하는 부분에 대한 코드는 이해가 되지 않은 상태로 넘기게 되었는데요. 만약 다른 코드를 보다 이해가 된다면 다시 찾아뵙도록 하겠습니다.
다음 포스트는 이제 구한 Residual을 따라 Jacobian Derivation을 통해 최적화를 진행하는 방법에 대해 다룰 예정입니다. 해당 내용 역시 이론과 코드 리뷰로 나눠서 진행할 테니 많은 관심 부탁 드립니다.
Reference
GitHub - JakobEngel/dso: Direct Sparse Odometry
Direct Sparse Odometry. Contribute to JakobEngel/dso development by creating an account on GitHub.
github.com
Computer Vision Group - Visual SLAM - DSO: Direct Sparse Odometry
DSO is a novel direct and sparse formulation for Visual Odometry. It combines a fully direct probabilistic model (minimizing a photometric error) with consistent, joint optimization of all model parameters, including geometry - represented as inverse depth
cvg.cit.tum.de
한 줄 요약
겁나 복잡하고 어렵다
Introduction
지난 포스트에서는 DSO의 Photometric Energy Term, 즉 Residual을 구하는 이론적 접근을 다뤘습니다. 이번 포스트에서는 이론을 실전으로 옮겨보도록 하겠습니다. 과연 어떻게 코드에서는 구현되어 있을까요?
일단 어디에 구현되어 있는지부터 봅시다. 일단 해당 내용은 CoarseInitializer.cc의 calcResAndGS() 함수에 구현되어 있습니다. 이는 DSO 초기화를 담당하는 구현부인데요. 초기화 영역 말고 실행 중에도 Residual은 계속 호출이 될 것입니다. 이는 CoarseTracker.cc의 calcRes() 함수에 구현되어 있으나, 이번 포스팅에서는 초기화 부분만 우선 살펴보도록 하겠습니다.
변수 초기화
우선 너무나 당연하게도 변수를 초기화해 줍니다. 여기서 초기화해 주는 변수들은 Residual을 구하기 위한 두 프레임과 관련되어 있습니다. 필요한 변수들이 무엇인지는 이전 포스트의 Notation 부분을 참고하면 알 수 있습니다. 아래는 해당 부분의 코드이며, 이중 RKi와 t는 꼭 기억해두는게 좋습니다. 각각
// CoarseInitializer::calcResAndGS()
// load width and height of image pyramid
int wl = w[lvl], hl = h[lvl];
// load Image of first and current frames
Eigen::Vector3f *colorRef = firstFrame->dIp[lvl];
Eigen::Vector3f *colorNew = newFrame->dIp[lvl];
// multiplex of rotation matrix and inverse matrix of intrinsic camera
// parameter
Mat33f RKi = (refToNew.rotationMatrix() * Ki[lvl]).cast<float>();
// translation matrix (first image -> current image)
Vec3f t = refToNew.translation().cast<float>();
// generate affine brightness transfer parameters
Eigen::Vector2f r2new_aff =
Eigen::Vector2f(exp(refToNew_aff.a), refToNew_aff.b);
// get intrinsic parameters of image pyramid
float fxl = fx[lvl];
float fyl = fy[lvl];
float cxl = cx[lvl];
float cyl = cy[lvl];
이제 초기화를 끝냈으면, 본격적으로 Residual을 구해봅시다. 점
이전 포스트에서 점
여기서
사실 이 부분에서 이상한 점이 하나 있는데요. 일단 다 살펴본 뒤 마지막에 해당 내용을 살펴보도록 하겠습니다.
// Reference: CoarseInitializer::calcResAndGS()
// iterate for residual pattern, patternNum is 8
for (int idx = 0; idx < patternNum; idx++) {
int dx = patternP[idx][0];
int dy = patternP[idx][1];
// calculate inner part of p' calculation, except multiplexing K
Vec3f pt =
RKi * Vec3f(point->u + dx, point->v + dy, 1) + t * point->idepth_new;
// normalize Z axis, to normalized plane (z=1)
float u = pt[0] / pt[2];
float v = pt[1] / pt[2];
// calculate new (u, v)
float Ku = fxl * u + cxl;
float Kv = fyl * v + cyl;
// update inverse depth
float new_idepth = point->idepth_new / pt[2];
if (!(Ku > 1 && Kv > 1 && Ku < wl - 2 && Kv < hl - 2 && new_idepth > 0)) {
isGood = false;
break;
}
여기서 patternP는 앞서 소개해드린 Residual Pattern입니다. SSE2 레지스터 사용을 위해 과감히 bottom-right을 제외시킨 과감한 패턴이었죠. 기억이 가물가물하다면 이전 포스트를 참고 부탁 드립니다. 아무튼 해당 패턴의 정보는 Settings.h/Setting.cc를 통해 확인할 수 있습니다. 왜 이름이 다른 거지...
// Reference: Settings.h
// use the ninth pattern (described in DSO's paper)
#define patternP staticPattern[8]
// Reference: Setting.cc
// 10 kinds of patterns
// -100 means "not used"
int staticPattern[10][40][2] = {
...
// the pattern used in dso's paper
/**
* |_|_|_|#|_|_|_|
* |_|_|#|_|#|_|_|
* |_|#|_|#|_|#|_|
* |_|_|#|_|_|_|_|
* |_|_|_|#|_|_|_|
*/
{{0, -2}, {-1, -1}, {1, -1}, {-2, 0}, {0, 0}, {2, 0}, {-1, 1}, {0, 2}, {-100, -100}, {-100, -100}, // 8 for SSE efficiency
{-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
{-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100},
{-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}, {-100, -100}},
...
};
Residual Modeling
이제 앞서 구한 정보들을 토대로 Residual을 구해봅시다. 이전 포스트에서 다룬 식에 대한 모델링을 다시 한번 보시면 좋을 것 같습니다. 다만 논문과 제가 정리한 글과도 다른 부분은 Huber Norm을 그대로 적용하지 않고, 좀 더 compact 하게 적용했다는 점입니다. 사실 이렇게 보니 직관적으로 이해하기가 쉽지 않기 때문에, 보편적인 표기법으로 Huber Norm을 적용했으면 어떨까 하는 마음이 있습니다. 간소화된 Huber weight와 Huber norm은 아래와 같습니다.
// Reference: CoarseInitializer::calcResAndGS()
// get intensity of p from first frame, p' from current frame
Vec3f hitColor = getInterpolatedElement33(colorNew, Ku, Kv, wl);
float rlR = getInterpolatedElement31(colorRef, point->u + dx, point->v + dy, wl);
// check the intensity whether it is acceptable or not
if (!std::isfinite(rlR) || !std::isfinite((float)hitColor[0])) {
isGood = false;
break;
}
// Photometric Energy Term Modeling
float residual = hitColor[0] - r2new_aff[0] * rlR - r2new_aff[1];
// Add Huber norm with compact huber weight
float hw = fabs(residual) < setting_huberTH
? 1
: setting_huberTH / fabs(residual);
// add energy H(r)*2
energy += hw * residual * residual * (2 - hw);
하나 특이한 점은 계산의 편의성인지 모르겠지만, 위에서 유도한
Question
앞서 코드에 이상한 점이 하나 있다고 했는데요. 혹시 기억하지 못하시면
아래 수식의 pt[2]로 pt[0], pt[1]을 나누어 u와 v를 구합니다. 즉, inverse depth를 두 번 곱하게 됩니다.
// Reference: CoarseInitializer::calcResAndGS()
// calculate inner part of p' calculation, except multiplexing K
Vec3f pt =
RKi * Vec3f(point->u + dx, point->v + dy, 1) + t * point->idepth_new;
// normalize Z axis, to normalized plane (z=1)
float u = pt[0] / pt[2];
float v = pt[1] / pt[2];
이런 계산식 설계 오류가 최종 결과에 방해가 되지 않을까요? 놀랍게도 답은 '아니다'입니다. 왜 아닐까요? 우리가 u, v를 구하기 위해 나누는 행위는 모두 1로 나누고 있다는 의미입니다. 아무런 의미 없는 행위이죠. 해당 부분을 코드에서 제거한다면, 나머지 연산에 대한 오버헤드가 줄어 속도 향상에 도움이 될지도 모르겠네요. 1로 나누는 것뿐이라 도움이 될지는 잘 모르겠습니다. 아래는 해당 pt[2]들을 출력한 결과물입니다. 전부 1을 나타내고 있죠.
Conclusion
이번 포스트에서는 DSO/LDSO에서 어떻게 Photometric Energy Term, Residual을 구현했는지 살펴봤습니다. 여러 트릭이 들어가 있다고 생각되었는데요. 개인적으로 꽤나 해석이 어려운 불친절한 코드 중 하나라고 생각합니다. 그래도 수식을 차근차근 따라가며 최적화 가능 포인트도 발굴했으니 꽤나 열심히 봤다고 생각합니다. 하지만 여전히 inverse depth를 업데이트하는 부분에 대한 코드는 이해가 되지 않은 상태로 넘기게 되었는데요. 만약 다른 코드를 보다 이해가 된다면 다시 찾아뵙도록 하겠습니다.
다음 포스트는 이제 구한 Residual을 따라 Jacobian Derivation을 통해 최적화를 진행하는 방법에 대해 다룰 예정입니다. 해당 내용 역시 이론과 코드 리뷰로 나눠서 진행할 테니 많은 관심 부탁 드립니다.
Reference
GitHub - JakobEngel/dso: Direct Sparse Odometry
Direct Sparse Odometry. Contribute to JakobEngel/dso development by creating an account on GitHub.
github.com
Computer Vision Group - Visual SLAM - DSO: Direct Sparse Odometry
DSO is a novel direct and sparse formulation for Visual Odometry. It combines a fully direct probabilistic model (minimizing a photometric error) with consistent, joint optimization of all model parameters, including geometry - represented as inverse depth
cvg.cit.tum.de
