현재 모듈화를 진행하지는 않았습니다. main함수에 함수를 구현해 놓은 상태입니다.
detect : 특징점을 찾습니다.
match : 두 이미지에서 검출된 특징점의 디스크립터를 비교하여 매칭합니다.
computeEssentialMatrix : 매칭된 특징점으로 이미지간 Essential Matrix를 계산합니다.
pixel2cam : 픽셀 좌표를 카메라 좌표로 전환합니다.
computeTriangulation : 특징점과 R|t 매트릭스를 이용하여 삼각 측량을 수행합니다.
SnavelyReprojectionError : 최적화를 위한 클래스입니다. Ceres 라이브러리에 포함된 템플릿 입니다.
optimization : 최적화를 수행하는 함수입니다. 최적화된 R|t 매트릭스를 반환합니다.
projection : 3D 좌표를 2D 좌표로 투영합니다. 투영된 좌표를 반환합니다.
visualization : 결과를 OpenCV로 이미지화 합니다.
Red : detected points
Green : projected points
[ befor optimization ]
[ after optimization ]
ceres-solver를 이용해서 최적화를 해보았다. 해당 이미지 상황은 KITTI 데이터에서 차량이 우회전 하는 상황이다.
먼저 삼각측량을 통해서 구현된 3d point를 projection 했을 때에는, 약간의 오차가 존재하였지만 reprojection이 잘 되었다. 이를 optimization하고자 하였다.
최적화 라이브러리는 ceres-solver를 사용하고, 최적화 방식은 doglet을 사용했는데, convergence가 떴는데도 오차가 매우 심하다. 원인이 무엇인지 잘 모르겠다. 어떻게 해결할 수 있을까?
ceres::Problem problem;
for (int i = 0; i < point2.size(); ++i)
{
double* point = new double[3]; // <-- 이 부분을 바깥으로 빼주니 결과가 달라지고 converge가 잘 되긴 한다. 하지만 최적화는 아직도 잘 이루어지지 않는 것 같다.
point[0] = point_3d[i].x;
point[1] = point_3d[i].y;
point[2] = point_3d[i].z;
ceres::CostFunction* cost_function =
SnavelyReprojectionError::Create(
point2[i].x,
point2[i].y);
problem.AddResidualBlock(cost_function,
new ceres::CauchyLoss(1.0),
camera,
point);
}
///////////////////////////////////////////////////////////
double* point = new double[3];
for (int i = 0; i < point2.size(); ++i)
{
...
}Three Views를 통해서 Optimization해보라는 의견에 구현을 하려고 보니(solvepnp를 사용하여), 결국엔 3D로 복원된 좌표와 마지막장에서 검출된 특징점과의 관계를 알아야만 solvepnp를 이용한 포즈 추정이 가능했다. 그래서 많은 SLAM들이 노드와 엣지를 구현하는 것 같다. 어떻게 구현해야할지...?
[ before optimization ]
[ after optimization ]
이번에 사용한 방식은 solvePnPRANSAC이다.
- 1번, 2번 이미지에서 SIFT를 이용하여 코너를 검출하고, 매칭을 시행한다.
- 매칭점을 이용하여 Essential Matrix를 구한다.
- 구한 E를 이용하여 R|t를 복원한다.
- 복원한 R|t를 이용하여 Triangulation을 진행한다.
- 이를 FramePoint로 만들어준다.
- 이미 구해진 2번 이미지의 키포인트와, SIFT를 이용하여 구해진 3번 이미지의 키포인트를 매칭시킨다.
- 이 때 중요한 점은, 3번 프레임과의 매칭점 중, 2번 이미지와 1번 이미지에서 모두 발견된 것을 골라내는 것이 중요하다. 그래야만 3D 포인트를 사용할 수 있다. 여기서는 unordered_map을 이용하여 query, train index 정보를 키값으로 저장하여, 이미 저장된 값이 있다면 이전 프레임과 연결되어 있는 것으로 판단하여 그때의 3D 포인트 값과, 3번 이미지에서의 2D값을 저장하여 반환한다.
- 이렇게 구해진 3D값과 2D 값을 이용하여 solvePnPRANSAC을 수행한다.
- R|t가 구해지면 projection을 수행한다.
- projection 후 음수 값이 나온 부분은 제외해준다.
- optimization을 수행한다.
- 최적화된 R|t가 구해지면 reprojection을 수행하여 결과를 비교한다.
여기까지가 수행한 단계인데, 결과가 좋지 못한 것 같다(참고로 가우시안 블러를 사용하면 결과가 더욱 좋지 않다). 무엇이 문제일까?
아래는 9번 과정의 결과이다.
origin : (578.908264, 134.374969)
projec : (590.978335, 50.718804)
=================
origin : (582.669189, 47.041477)
projec : (585.276824, 57.971245)
=================
origin : (587.556213, 199.231354)
projec : (609.899974, 15.830458)
=================
origin : (594.593689, 195.211761)
projec : (611.707643, 139.005742)
=================
origin : (633.860413, 42.455711)
projec : (639.182287, 140.319481)
=================
origin : (683.031616, 252.503296)
projec : (684.175607, 67.605407)
=================
origin : (836.625183, 121.790894)
projec : (822.458695, 26.432818)
=================
origin : (847.748901, 85.347565)
projec : (848.809171, 89.596108)
=================
origin : (852.450928, 42.482689)
projec : (851.619501, 76.445709)
=================
origin : (853.255493, 119.849640)
projec : (852.588645, 39.251664)
=================
origin : (855.063110, 82.011162)
projec : (851.984296, 121.154142)
=================
origin : (869.929932, 201.187637)
projec : (888.623114, 3.573617)
=================
origin : (874.568848, 71.583290)
projec : (871.608026, 122.891977)
=================
origin : (936.848389, 83.042290)
projec : (874.888399, 93.481893)
=================
origin : (917.836182, 48.588970)
projec : (911.431005, 120.343448)
=================
origin : (920.598145, 11.395925)
projec : (923.298345, 21.510419)
=================
origin : (928.673828, 12.016891)
projec : (928.099091, 119.196268)
=================
origin : (941.422607, 72.585846)
projec : (944.684881, 74.358661)
=================
origin : (943.274658, 12.255217)
projec : (950.356025, 53.317886)
=================
origin : (1020.919983, 83.480324)
projec : (949.715524, 78.884759)
=================
origin : (948.909851, 119.259819)
projec : (949.589997, 117.903777)
=================
origin : (954.390137, 210.764450)
projec : (952.623403, 212.494818)
=================
origin : (948.271423, 72.626534)
projec : (959.707096, 60.484122)
=================
origin : (968.226257, 233.290222)
projec : (959.954769, 90.355036)
=================
origin : (964.160034, 53.334892)
projec : (960.307541, 40.068695)
=================
origin : (964.795471, 65.467781)
projec : (963.660989, 15.583593)
=================
origin : (984.885559, 53.966370)
projec : (973.339674, 52.878325)
=================
origin : (989.585876, 111.848587)
projec : (977.322683, 86.597054)
=================
origin : (1039.052612, 73.643143)
projec : (979.106628, 81.713593)
=================
origin : (993.562988, 83.276428)
projec : (982.335849, 72.285876)
=================
origin : (999.349365, 197.113129)
projec : (992.238608, 88.980394)
=================
origin : (1003.682800, 45.244202)
projec : (1016.817054, 199.167062)
=================
origin : (1003.931519, 193.559113)
projec : (1002.499926, 6.062514)
=================
origin : (936.848389, 83.042290)
projec : (1007.241384, 90.452512)
=================
origin : (1011.660034, 73.369102)
projec : (1005.897817, 63.266508)
=================
origin : (964.824951, 82.967209)
projec : (1009.125679, 71.075490)
=================
origin : (1026.598511, 258.425903)
projec : (1016.704598, 57.606881)
=================
origin : (1021.540955, 14.816463)
projec : (1016.704598, 57.606881)
=================
origin : (1026.696655, 159.377090)
projec : (1019.971928, 70.379438)
=================아래는 12번 과정의 결과이다.
origin : (578.908264, 134.374969)
projec : (192.195966, 150.647267)o
=================
origin : (582.669189, 47.041477)
projec : (252.176844, 125.635611)o
=================
origin : (587.556213, 199.231354)
projec : (123.724393, 153.744957)o
=================
origin : (594.593689, 195.211761)
projec : (235.907797, 238.998100)o
=================
origin : (633.860413, 42.455711)
projec : (230.188786, 254.855279)o
=================
origin : (683.031616, 252.503296)
projec : (223.016157, 194.846199)o
=================
origin : (836.625183, 121.790894)
projec : (239.814035, 197.597183)o
=================
origin : (847.748901, 85.347565)
projec : (280.659747, 254.156712)o
=================
origin : (852.450928, 42.482689)
projec : (280.462664, 241.281297)o
=================
origin : (853.255493, 119.849640)
projec : (270.292433, 207.316824)o
=================
origin : (855.063110, 82.011162)
projec : (292.479949, 282.671544)o
=================
origin : (869.929932, 201.187637)
projec : (-317.804252, 491.207168)o
=================
origin : (874.568848, 71.583290)
projec : (315.114245, 279.444321)o
=================
origin : (936.848389, 83.042290)
projec : (310.224013, 252.188102)o
=================
origin : (917.836182, 48.588970)
projec : (360.897396, 266.211534)o
=================
origin : (920.598145, 11.395925)
projec : (-42.282539, 380.611994)o
=================
origin : (928.673828, 12.016891)
projec : (376.788256, 262.245417)o
=================
origin : (941.422607, 72.585846)
projec : (383.994750, 218.036121)o
=================
origin : (943.274658, 12.255217)
projec : (392.170566, 194.125735)o
=================
origin : (1020.919983, 83.480324)
projec : (422.795140, 203.657475)o
=================
origin : (948.909851, 119.259819)
projec : (399.503714, 256.028065)o
=================
origin : (954.390137, 210.764450)
projec : (1323.570468, -158.218684)o
=================
origin : (948.271423, 72.626534)
projec : (401.547181, 199.794168)o
=================
origin : (968.226257, 233.290222)
projec : (404.804071, 228.600348)o
=================
origin : (964.160034, 53.334892)
projec : (160.628908, 307.252024)o
=================
origin : (964.795471, 65.467781)
projec : (391.306095, 160.856178)o
=================
origin : (984.885559, 53.966370)
projec : (322.402698, 239.150205)o
=================
origin : (989.585876, 111.848587)
projec : (418.602100, 223.362306)o
=================
origin : (1039.052612, 73.643143)
projec : (418.648383, 219.006827)o
=================
origin : (993.562988, 83.276428)
projec : (422.682097, 208.418145)o
=================
origin : (999.349365, 197.113129)
projec : (436.882108, 221.025431)o
=================
origin : (1003.682800, 45.244202)
projec : (1235.462015, -103.600852)o
=================
origin : (1003.931519, 193.559113)
projec : (428.984943, 145.446243)o
=================
origin : (936.848389, 83.042290)
projec : (577.193307, 151.773917)o
=================
origin : (1011.660034, 73.369102)
projec : (443.003992, 196.581516)o
=================
origin : (964.824951, 82.967209)
projec : (450.847048, 201.293493)o
=================
origin : (1026.598511, 258.425903)
projec : (456.777695, 187.249249)o
=================
origin : (1021.540955, 14.816463)
projec : (456.777695, 187.249249)o
=================
origin : (1026.696655, 159.377090)
projec : (461.540669, 198.536671)o
=================문제점 발견 : 모든 포인트가 최적화 되면 좌측으로 쏠리는 현상이 발생했다. 분명히 식의 문제라고 원인 진단을 하고 자세히 살펴보니, 기존 ceres에서 제공하던 템플릿은 x,y,theta일 때 사용하던 템플릿이다. 이 프로젝트는 x,y,z 값을 사용하기에 값이 자꾸 이상하게 나왔던 것. 해당 템플릿을 직접 이미지 좌표로 변환하는 최적화 식 작성(그러나 아래 식은 파라미터가 15개여서 R(9개)값을 rodrigues(3개)로 넘겨줄 필요가 있다).
struct SnavelyReprojectionError
{
SnavelyReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
template <typename T>
bool operator()(const T *const R,
const T *const t,
const T *const p,
T *residuals) const
{
T origin_x = T(p[0]);
T origin_y = T(p[1]);
T origin_z = T(p[2]);
T cam_x = R[0] * origin_x + R[1] * origin_y + R[2] * origin_z + t[0];
T cam_y = R[3] * origin_x + R[4] * origin_y + R[5] * origin_z + t[1];
T cam_z = R[6] * origin_x + R[7] * origin_y + R[8] * origin_z + t[2];
T proj_x = (cam_x * k[0] + cam_z * k[2]) / cam_z;
T proj_y = (cam_y * k[4] + cam_z * k[5]) / cam_z;
T diff_x = proj_x - T(observed_x);
T diff_y = proj_y - T(observed_y);
residuals[0] = diff_x * diff_x;
residuals[1] = diff_y * diff_y;
return true;
}[ before optimization ]
[ after optimization ]
실제로 얼마나 최적화된지는 모르겠지만, 수치상으로는 에러 값이 낮아졌다.
이제 다음으로 진행해야 할 부분은, T13, T12를 이용하여 T23을 구하고, T23으로 triangulation을 수행하여 3D 값을 복원한 후, 이를 T31로 곱하여 월드좌표로 만들어주어 FramePoint로 저장만 하면 될 것 같다. 이 과정이 반복되면 Visual Odometry + Pose Optimization까지는 가능할 듯하다.