1. trackFeatures Function

1. predictFeatureTracking

predictFeatureTracking 함수는 이전 프레임에서 현재 프레임으로의 특징점 위치를 예측하기 위해 사용됩니다. 이 함수는 두 프레임 사이의 회전 변화를 보상하여, 현재 프레임에서의 특징점 위치를 예측합니다. 이를 통해 광학 흐름 추적과 같은 특징 추적 알고리즘이 초기값을 가지고 시작할 수 있도록 도와줍니다. 이 과정은 추적 알고리즘의 안정성과 정확성을 향상시키는 데 중요합니다.

predictFeatureTracking 함수 분석

이 함수의 전체적인 역할은 IMU 데이터를 이용해 이전 프레임에서 현재 프레임으로 회전 보정을 수행하고, 보정된 특징점 좌표를 계산하는 것입니다. IMU 데이터를 활용하여 회전 보상을 하면, 카메라가 빠르게 회전하는 경우에도 특징점의 위치를 정확히 추적하는 데 도움이 됩니다.

함수 정의

void ImageProcessor::predictFeatureTracking(
    const vector<cv::Point2f>& input_pts,
    const cv::Matx33f& R_p_c,
    const cv::Vec4d& intrinsics,
    vector<cv::Point2f>& compensated_pts) {

• input_pts: 이전 프레임에서 검출된 특징점의 좌표입니다.
• R_p_c: 이전 프레임에서 현재 프레임으로의 회전 보정을 나타내는 회전 행렬입니다. 이 행렬은 integrateImuData 함수를 통해 계산됩니다.
• intrinsics: 카메라의 내부 파라미터입니다. [fx, fy, cx, cy]의 형태로 되어 있으며, 초점 거리(focal length)와 주점(principal point)을 포함합니다.
• compensated_pts: 회전이 보정된 후 현재 프레임에서 예측된 특징점의 좌표입니다.

1. 입력 특징점이 없을 경우 처리

if (input_pts.size() == 0) {
    compensated_pts.clear();
    return;
}
compensated_pts.resize(input_pts.size());

• 만약 입력된 특징점이 없다면, 함수는 보정을 위한 작업을 수행할 필요가 없습니다. 따라서 보정된 특징점 벡터를 초기화하고 함수를 종료합니다.
• 입력된 특징점이 있다면, 출력 벡터 (compensated_pts)의 크기를 입력 벡터 (input_pts)와 동일하게 설정합니다.

2. 카메라 행렬 (Intrinsic Matrix) 설정

cv::Matx33f K(
    intrinsics[0], 0.0, intrinsics[2],
    0.0, intrinsics[1], intrinsics[3],
    0.0, 0.0, 1.0);

• 카메라의 내재 행렬 (K)을 생성합니다. 내재 행렬은 다음과 같은 형태로 구성됩니다.

  $K = \begin{bmatrix} fx & 0 & cx \\ 0 & fy & cy \\ 0 & 0 & 1 \end{bmatrix}$

  여기서 fx, fy는 초점 거리이고, cx, cy는 주점입니다. 내재 행렬은 이미지의 픽셀 좌표를 정규화된 좌표로 변환하거나, 반대로 정규화된 좌표를 이미지 좌표로 변환하는 데 사용됩니다.

3. 보정된 투영 행렬 계산

cv::Matx33f H = K * R_p_c * K.inv();

• H는 보정된 투영 행렬 (Homography Matrix)입니다.
  • 먼저, 이전 프레임에서 현재 프레임으로의 회전 행렬 R_p_c와 내재 행렬 K를 사용하여 H를 계산합니다.
  • 이 행렬은 두 프레임 간의 특징점의 이동을 예측하기 위해 사용되며, K * R_p_c * K.inv() 형태로 계산됩니다.
  • K.inv()를 통해 정규화된 좌표계를 적용한 후 R_p_c로 회전 보정을 수행하고 다시 내재 행렬 K를 적용하여 이미지 좌표로 변환합니다.

4. 특징점의 위치 보정

for (int i = 0; i < input_pts.size(); ++i) {
    cv::Vec3f p1(input_pts[i].x, input_pts[i].y, 1.0f);
    cv::Vec3f p2 = H * p1;
    compensated_pts[i].x = p2[0] / p2[2];
    compensated_pts[i].y = p2[1] / p2[2];
}

• 이전 프레임의 특징점 input_pts를 호모지니어스 좌표계로 확장합니다. 이 좌표계에서는 점을 [x, y, 1]^T 형태로 표현합니다.
  • cv::Vec3f p1(input_pts[i].x, input_pts[i].y, 1.0f)는 특징점을 호모지니어스 좌표로 변환한 것입니다.
• p2 = H * p1을 통해 투영 행렬 H를 사용해 현재 프레임에서 예측된 위치 p2를 계산합니다.
• compensated_pts[i].x와 compensated_pts[i].y는 보정된 특징점의 좌표입니다. 호모지니어스 좌표를 보통의 2D 좌표로 변환하기 위해 p2[2]로 나누어줍니다 (p2는 [x', y', w']^T 형태이므로, 실제 좌표는 [x'/w', y'/w'] 형태가 됩니다).

요약

predictFeatureTracking 함수는 IMU 데이터를 이용해 두 프레임 간의 회전을 보정하고, 이를 통해 이전 프레임의 특징점 위치를 현재 프레임에서 예측합니다. 이를 통해 Optical Flow와 같은 특징 추적 알고리즘의 초기값을 제공하며, 이는 특징 추적의 정확도와 속도를 향상시킵니다.

2. calcOpticalFlowPyrLK

이 코드는 OpenCV 라이브러리에서 제공하는 calcOpticalFlowPyrLK 함수를 사용하여 이전 프레임에서 현재 프레임으로 Lucas-Kanade Optical Flow를 계산하는 부분입니다. 이 함수는 두 개의 연속적인 프레임 간에 특징점을 추적하여 움직임을 계산합니다. 특징 추적은 로봇 비전 및 컴퓨터 비전 응용 프로그램에서 매우 중요하며, 이 함수는 그러한 특징 추적 작업을 수행합니다.

calcOpticalFlowPyrLK 함수 설명

cv::calcOpticalFlowPyrLK(
    prev_cam0_pyramid_, curr_cam0_pyramid_,
    prev_cam0_points, curr_cam0_points,
    track_inliers, noArray(),
    Size(processor_config.patch_size, processor_config.patch_size),
    processor_config.pyramid_levels,
    TermCriteria(TermCriteria::COUNT + TermCriteria::EPS,
                 processor_config.max_iteration,
                 processor_config.track_precision),
    cv::OPTFLOW_USE_INITIAL_FLOW);

함수 인자 분석

1. prev_cam0_pyramid_와 curr_cam0_pyramid_:

   • 이 인자는 이전 프레임과 현재 프레임의 이미지 피라미드입니다.
   • 이미지 피라미드는 다양한 스케일에서 특징을 찾을 수 있도록 이미지를 점점 축소한 레벨로 구성된 구조입니다.
   • 피라미드 레벨을 사용하면 멀티 스케일에서 추적을 시도하므로, 특징이 이미지에서 크게 이동해도 추적 성공률이 높아집니다.
   • 여기서는 prev_cam0_pyramid_가 이전 프레임의 피라미드이며, curr_cam0_pyramid_가 현재 프레임의 피라미드입니다.

2. prev_cam0_points와 curr_cam0_points:

   • prev_cam0_points는 이전 프레임에서 추적하고자 하는 특징점의 좌표 목록입니다.
   • curr_cam0_points는 현재 프레임에서 추적된 결과가 저장될 특징점의 좌표 목록입니다.
   • 이 curr_cam0_points는 초기 위치 추정치를 포함하고 있으며, 이를 기반으로 최종 추적된 위치를 갱신합니다.
   • 추적이 실패한 경우에는 curr_cam0_points는 추적 실패로 처리됩니다.

3. track_inliers:

   • 이 벡터는 추적 성공 여부를 표시합니다. unsigned char 형식의 값을 가지며, 특징점이 성공적으로 추적되었는지 여부를 1(성공) 또는 0(실패)로 나타냅니다.
   • Optical Flow가 계산된 후, 이 벡터에서 추적이 성공한 점만 남기고 나머지는 제외하는 작업을 합니다.

4. noArray():

   • 두 번째 출력 인자는 계산된 광학 흐름의 오류를 저장하는 매트릭스입니다.
   • 여기서는 noArray()를 사용하여 이 정보를 사용하지 않겠다는 의미입니다. 오류 정보가 필요하지 않을 때, 메모리를 절약하고 계산을 단순화하기 위해 noArray()로 지정합니다.

5. Size(processor_config.patch_size, processor_config.patch_size):

   • 검색 윈도우의 크기를 설정하는 부분입니다.
   • Lucas-Kanade Optical Flow는 특징점을 찾기 위해 설정된 크기의 영역을 검사합니다. 여기서는 patch_size x patch_size 크기의 윈도우를 사용하여 해당 범위 내에서 추적을 시도합니다.
   • 윈도우 크기는 추적의 성공률에 영향을 미칠 수 있으며, 너무 작으면 큰 이동을 찾지 못할 수 있고, 너무 크면 잡음이나 연산 시간이 늘어날 수 있습니다.

6. processor_config.pyramid_levels:

   • 이미지 피라미드의 레벨 수입니다.
   • 이 값은 얼마나 많은 스케일에서 이미지를 줄여서 추적을 할 것인지 결정합니다.
   • 피라미드 레벨이 높을수록 더 많은 스케일을 다루게 되므로 추적에 유리하지만, 연산 시간이 길어질 수 있습니다.

7. TermCriteria:

   • 종료 조건을 설정하는 부분입니다. Optical Flow 계산이 언제 멈출 것인지 정하는 기준입니다.
   • TermCriteria::COUNT + TermCriteria::EPS: 이 부분은 반복 횟수와 에러 임곗값을 모두 고려하여 종료 조건을 설정하겠다는 의미입니다.
   • processor_config.max_iteration: 최대 반복 횟수를 의미합니다. 설정된 횟수만큼 반복하면서 최적의 결과를 찾게 됩니다.
   • processor_config.track_precision: 추적의 정확도를 설정하는 부분입니다. 이 값을 기준으로 현재 반복에서의 위치 오차가 충분히 작아지면 반복을 종료합니다.

8. cv::OPTFLOW_USE_INITIAL_FLOW:

   • 이 플래그는 Optical Flow 알고리즘이 추적을 시작할 때 초기 위치를 사용할 것인지 여부를 나타냅니다.
   • 이 경우에는 curr_cam0_points에 이미 초기 위치가 설정되어 있으며, Optical Flow 알고리즘은 이 초기 위치를 기반으로 최적화를 수행합니다.
   • 이 초기 위치는 IMU 데이터를 사용하여 회전 보정 후 예측한 결과로 사용됩니다. 이렇게 초기 위치를 제공함으로써 추적의 정확성과 효율성을 높일 수 있습니다.

3. 추적된 특징점 정리 (removeUnmarkedElements)

removeUnmarkedElements(prev_ids, track_inliers, prev_tracked_ids);
removeUnmarkedElements(prev_lifetime, track_inliers, prev_tracked_lifetime);
removeUnmarkedElements(prev_cam0_points, track_inliers, prev_tracked_cam0_points);
removeUnmarkedElements(prev_cam1_points, track_inliers, prev_tracked_cam1_points);
removeUnmarkedElements(curr_cam0_points, track_inliers, curr_tracked_cam0_points);

• removeUnmarkedElements 함수는 주어진 벡터에서 track_inliers 값이 1로 설정된 요소들만 남기고, 나머지를 제거합니다.

• 이 함수는 추적에 성공한 특징점들만을 남기기 위한 필터링 작업입니다.

• prev_tracked_ids, prev_tracked_lifetime, prev_tracked_cam0_points, prev_tracked_cam1_points, curr_tracked_cam0_points와 같은 벡터들에 대해, 성공적으로 추적된 점들만 남기도록 필터링을 수행합니다.

3.1 스테레오 매칭

vector<Point2f> curr_cam1_points(0);
vector<unsigned char> match_inliers(0);
stereoMatch(curr_tracked_cam0_points, curr_cam1_points, match_inliers);

• stereoMatch 함수는 현재 프레임에서의 왼쪽 카메라(cam0)의 특징점과 오른쪽 카메라(cam1)의 특징점을 매칭합니다. match_inliers는 각 특징점에 대해 스테레오 매칭이 성공했는지를 나타내며, 성공한 경우 1, 실패한 경우 0입니다.

3.2 스테레오 매칭 결과 정리

removeUnmarkedElements(prev_tracked_ids, match_inliers, prev_matched_ids);
removeUnmarkedElements(prev_tracked_lifetime, match_inliers, prev_matched_lifetime);
removeUnmarkedElements(prev_tracked_cam0_points, match_inliers, prev_matched_cam0_points);
removeUnmarkedElements(prev_tracked_cam1_points, match_inliers, prev_matched_cam1_points);
removeUnmarkedElements(curr_tracked_cam0_points, match_inliers, curr_matched_cam0_points);
removeUnmarkedElements(curr_cam1_points, match_inliers, curr_matched_cam1_points);

• 스테레오 매칭이 성공한 점들만을 남기도록 removeUnmarkedElements 함수를 다시 사용합니다.이로써 이전 프레임과 현재 프레임의 왼쪽/오른쪽 카메라의 특징점이 스테레오 매칭이 성공한 점으로만 정리됩니다.이 단계에서는 스테레오 매칭이 실패한 점들은 이후 처리에서 제외됩니다.

3.3 RANSAC 기반 Outlier 제거

vector<int> cam0_ransac_inliers(0);
twoPointRansac(prev_matched_cam0_points, curr_matched_cam0_points,
    cam0_R_p_c, cam0_intrinsics, cam0_distortion_model,
    cam0_distortion_coeffs, processor_config.ransac_threshold,
    0.99, cam0_ransac_inliers);

vector<int> cam1_ransac_inliers(0);
twoPointRansac(prev_matched_cam1_points, curr_matched_cam1_points,
    cam1_R_p_c, cam1_intrinsics, cam1_distortion_model,
    cam1_distortion_coeffs, processor_config.ransac_threshold,
    0.99, cam1_ransac_inliers);

• RANSAC (Random Sample Consensus)은 외란(outlier)을 제거하는 강력한 알고리즘으로, 두 프레임 간에 일치하지 않는 잘못된 매칭을 제거하는 데 사용됩니다.

• Step 2와 Step 3에서 RANSAC을 사용하여 각각 왼쪽 카메라(cam0)와 오른쪽 카메라(cam1)의 두 프레임 간의 특징점을 검증합니다.

• cam0_ransac_inliers와 cam1_ransac_inliers는 RANSAC을 통과한 특징점에 대해 1로 표시되고, 그렇지 않으면 0으로 표시됩니다.

3.4 RANSAC 결과 정리 및 저장

for (int i = 0; i < cam0_ransac_inliers.size(); ++i) {
    if (cam0_ransac_inliers[i] == 0 || cam1_ransac_inliers[i] == 0) continue;
    int row = static_cast<int>(curr_matched_cam0_points[i].y / grid_height);
    int col = static_cast<int>(curr_matched_cam0_points[i].x / grid_width);
    int code = row * processor_config.grid_col + col;
    (*curr_features_ptr)[code].push_back(FeatureMetaData());

    FeatureMetaData& grid_new_feature = (*curr_features_ptr)[code].back();
    grid_new_feature.id = prev_matched_ids[i];
    grid_new_feature.lifetime = ++prev_matched_lifetime[i];
    grid_new_feature.cam0_point = curr_matched_cam0_points[i];
    grid_new_feature.cam1_point = curr_matched_cam1_points[i];

    ++after_ransac;
}

• RANSAC 결과에서 양쪽 카메라(cam0과 cam1) 모두에서 유효한 특징점들만 남깁니다.
  각 특징점을 그리드에 할당하여 관리합니다. 이를 통해 특정 영역에서 고르게 특징점들이 분포하도록 합니다.
• FeatureMetaData 구조체를 사용하여 현재 프레임의 특징 정보를 저장하며, 특징의 수명(lifetime)을 증가시킵니다. after_ransac 변수에 RANSAC을 통과한 특징점의 수를 기록합니다.

4.stereoMatch

stereoMatch 함수는 스테레오 카메라 설정에서 특징점을 매칭하기 위해 사용하는 함수입니다. 이 함수는 왼쪽 카메라(cam0)와 오른쪽 카메라(cam1)의 이미지에서 같은 물리적 점을 찾기 위해 Optical Flow 및 에센셜 행렬을 이용한 정밀 매칭을 수행합니다. 이를 통해 두 카메라가 동일한 특징을 관찰할 수 있도록 합니다.

함수 분석

이 함수의 주된 목적은 왼쪽 (cam0) 카메라에서 추적된 특징점들을 오른쪽 (cam1) 카메라 이미지에서 찾는 것입니다. 이를 위해 Optical Flow와 기하학적 제약을 활용하여 신뢰할 수 있는 매칭을 수행합니다.

함수 인자

• cam0_points: 왼쪽 카메라(cam0)의 이미지에서 얻은 특징점 좌표.
• cam1_points: 오른쪽 카메라(cam1)의 이미지에서 매칭된 특징점 좌표를 저장할 벡터. 이 벡터는 Optical Flow 초기값을 가지거나, 비어 있을 수 있습니다.
• inlier_markers: 매칭된 특징점이 유효한지 (1) 또는 유효하지 않은지 (0)를 표시하는 벡터.

주요 단계

1. 기본 조건 확인 및 초기화:

   • 입력된 특징점이 없다면 함수는 바로 반환됩니다.
   • cam1_points가 초기화되지 않은 경우, 초기 위치를 추정하여 시작점으로 설정합니다.

2. Optical Flow를 이용한 특징 매칭:

   • calcOpticalFlowPyrLK 함수는 Lucas-Kanade 방법을 사용하여 두 프레임 사이의 특징점을 추적합니다.
   • 왼쪽 카메라 이미지의 피라미드와 오른쪽 카메라 이미지의 피라미드를 입력으로 받아 Optical Flow를 계산하고, 매칭 결과를 inlier_markers에 저장합니다.

3. 이미지 경계를 벗어난 특징점 필터링:

   • Optical Flow를 통해 매칭된 점들 중에서 이미지 경계를 벗어난 점들을 필터링하여 inlier_markers에서 제거합니다.

4. 에센셜 행렬을 이용한 추가적인 outlier 제거:

   • 에센셜 행렬을 계산하고, 이를 사용해 에피폴라 제약 조건에 의해 제거할 점들을 식별합니다.
   • 이를 통해 두 프레임 간 기하학적으로 일관되지 않는 점들을 걸러냅니다.

세부 단계 설명

1. 초기화 및 특징점 투영

if (cam0_points.size() == 0) return;

if(cam1_points.size() == 0) {
    // Initialize cam1_points by projecting cam0_points to cam1 using the
    // rotation from stereo extrinsics
    const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
    vector<cv::Point2f> cam0_points_undistorted;
    undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model,
                    cam0_distortion_coeffs, cam0_points_undistorted,
                    R_cam0_cam1);
    cam1_points = distortPoints(cam0_points_undistorted, cam1_intrinsics,
                                cam1_distortion_model, cam1_distortion_coeffs);
}

• 만약 cam1_points가 비어 있다면, cam0_points를 사용해 초기 추정치를 생성합니다.
  • undistortPoints를 통해 cam0_points를 왜곡 보정한 후, 스테레오 설정에서 회전 행렬을 사용해 cam1으로 투영합니다.
  • 이렇게 추정된 점들을 cam1_points의 초기값으로 사용하여 Optical Flow 추적에 사용합니다.

2. Optical Flow를 사용한 특징점 추적

calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_,
    cam0_points, cam1_points,
    inlier_markers, noArray(),
    Size(processor_config.patch_size, processor_config.patch_size),
    processor_config.pyramid_levels,
    TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,
                 processor_config.max_iteration,
                 processor_config.track_precision),
    cv::OPTFLOW_USE_INITIAL_FLOW);

• calcOpticalFlowPyrLK 함수는 두 프레임 사이에서 특징점을 추적하기 위해 Lucas-Kanade Optical Flow를 사용합니다.
• Optical Flow를 통해 왼쪽 카메라의 특징점(cam0_points)을 기반으로 오른쪽 카메라의 특징점(cam1_points)을 추정합니다.
• inlier_markers는 Optical Flow가 성공했는지 여부를 나타내는 플래그입니다.

3. 이미지 경계를 벗어난 점들 제거

for (int i = 0; i < cam1_points.size(); ++i) {
    if (inlier_markers[i] == 0) continue;
    if (cam1_points[i].y < 0 ||
        cam1_points[i].y > cam1_curr_img_ptr->image.rows-1 ||
        cam1_points[i].x < 0 ||
        cam1_points[i].x > cam1_curr_img_ptr->image.cols-1)
      inlier_markers[i] = 0;
}

• Optical Flow를 통해 추적된 특징점들 중에서 이미지의 경계를 벗어난 점들은 유효하지 않으므로 제거합니다.
• 이러한 점들을 inlier_markers[i] = 0으로 설정하여 이후 처리에서 제외합니다.

4. 에센셜 행렬을 사용한 outlier 제거

4.1 에센셜 행렬 계산

const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
const cv::Vec3d t_cam0_cam1 = R_cam1_imu.t() * (t_cam0_imu - t_cam1_imu);
// Compute the essential matrix.
const cv::Matx33d t_cam0_cam1_hat(
    0.0, -t_cam0_cam1[2], t_cam0_cam1[1],
    t_cam0_cam1[2], 0.0, -t_cam0_cam1[0],
    -t_cam0_cam1[1], t_cam0_cam1[0], 0.0);
const cv::Matx33d E = t_cam0_cam1_hat * R_cam0_cam1;

• 에센셜 행렬(E)은 두 카메라 간의 기하학적 관계를 설명하는 행렬로, 두 카메라 간의 상대적인 회전과 이동을 결합한 것입니다.
• R_cam0_cam1은 왼쪽(cam0)에서 오른쪽(cam1) 카메라로의 회전을 나타내며, t_cam0_cam1은 이동을 나타냅니다.
• t_cam0_cam1_hat은 벡터 t_cam0_cam1의 반대칭 행렬로, 벡터 곱을 에센셜 행렬에 포함하기 위해 사용됩니다.
• 최종적으로 에센셜 행렬(E)은 두 카메라의 회전과 이동을 결합하여 정의됩니다.

4.2 에피폴라 제약을 통한 outlier 제거

vector<cv::Point2f> cam0_points_undistorted(0);
vector<cv::Point2f> cam1_points_undistorted(0);
undistortPoints(
    cam0_points, cam0_intrinsics, cam0_distortion_model,
    cam0_distortion_coeffs, cam0_points_undistorted);
undistortPoints(
    cam1_points, cam1_intrinsics, cam1_distortion_model,
    cam1_distortion_coeffs, cam1_points_undistorted);

double norm_pixel_unit = 4.0 / (
    cam0_intrinsics[0] + cam0_intrinsics[1] +
    cam1_intrinsics[0] + cam1_intrinsics[1]);

for (int i = 0; i < cam0_points_undistorted.size(); ++i) {
    if (inlier_markers[i] == 0) continue;
    cv::Vec3d pt0(cam0_points_undistorted[i].x,
                  cam0_points_undistorted[i].y, 1.0);
    cv::Vec3d pt1(cam1_points_undistorted[i].x,
                  cam1_points_undistorted[i].y, 1.0);
    cv::Vec3d epipolar_line = E * pt0;
    double error = fabs((pt1.t() * epipolar_line)[0]) / sqrt(
        epipolar_line[0] * epipolar_line[0] + epipolar_line[1] * epipolar_line[1]);
    if (error > processor_config.stereo_threshold * norm_pixel_unit)
        inlier_markers[i] = 0;
}

• 에피폴라 제약 조건을 사용해 추가적으로 outlier를 제거합니다.
  • undistortPoints 함수를 사용하여 카메라의 내부 왜곡을 보정합니다.
• 두 카메라 간의 특징점이 에센셜 행렬에 의해 정의된 에피폴라 선(epipolar line)에 얼마나 가까이 위치하는지를 계산합니다.
  • 에피폴라 선은 카메라 간의 움직임에 따라 각 점이 투영될 수 있는 위치를 나타냅니다.
  • 계산된 에러(error)가 미리 정의된 임계값(stereo_threshold * norm_pixel_unit)을 초과하면, 그 특징점은 기하학적 일관성이 없는 것으로 간주하고 inlier_markers[i]를 0으로 설정합니다.

요약

stereoMatch 함수는 두 개의 카메라 간에 동일한 특징점을 찾기 위해 Optical Flow와 에센셜 행렬 기반 outlier 제거를 결합한 방식으로 특징점을 매칭합니다. 주요 작업은 다음과 같습니다.

1. 초기 추정:

   • 만약 cam1_points가 비어 있을 경우, cam0_points를 회전과 내재 행렬을

   사용해 오른쪽 카메라의 초기 위치로 투영합니다.

2. Optical Flow 사용:

   • Lucas-Kanade Optical Flow를 통해 왼쪽 카메라에서 찾은 특징점이 오른쪽 카메라에서 어디로 이동했는지 계산합니다.

3. 이미지 경계 벗어난 점 필터링:

   • Optical Flow 결과 중 이미지 경계를 벗어난 점들을 필터링하여 제거합니다.

4. 에센셜 행렬을 이용한 추가적인 검증:

   • 에센셜 행렬을 이용해 에피폴라 제약을 적용함으로써 잘못 매칭된 점(outlier)을 추가적으로 제거합니다.
   • 두 카메라 간의 기하학적 관계를 유지하는 특징점만 남기므로, 매칭의 신뢰성이 높아집니다.

이와 같은 과정을 통해 두 카메라 간의 특징점 매칭을 수행하게 되고, 결과적으로 스테레오 비전에서 일치하는 물체의 3D 정보를 더 정확하게 추정할 수 있게 됩니다.

5. AddNewFeatures

addNewFeatures 함수는 이미지에 새로 등장하는 특징점들을 추출하여 현재 프레임에 추가하는 역할을 합니다. 이 함수는 이미 추적된 특징점들을 제외하고 새로운 특징점들을 이미지의 빈 영역에 추가함으로써 프레임 간 일관성 있게 특징점을 유지합니다. 특히, 이미지의 각 영역이 적절히 커버되도록 그리드 구조를 사용하여 균형 잡힌 특징점을 얻는 것이 이 함수의 목표입니다.

함수의 주요 단계

1. 이미지 마스크 생성: 기존의 특징점이 있는 영역을 피하기 위해 마스크 생성.
2. 새로운 특징점 탐지: 기존 특징점이 없는 영역에서 새로운 특징점을 탐지.
3. 그리드 기반 특징점 분할 및 필터링: 그리드 기반으로 특징점을 나누고, 각 그리드에서 특정 개수의 특징점만 선택.
4. 스테레오 매칭: 새로운 특징점을 오른쪽 카메라 이미지와 매칭하여 유효한 특징점만 남김.
5. 그리드에 특징점 추가: 현재 프레임에 추적되지 않은 그리드에 새로운 특징점을 추가하여 전체적으로 고르게 분포되도록 함.

코드 분석

1. 이미지 마스크 생성

const Mat& curr_img = cam0_curr_img_ptr->image;

// Size of each grid.
static int grid_height = cam0_curr_img_ptr->image.rows / processor_config.grid_row;
static int grid_width = cam0_curr_img_ptr->image.cols / processor_config.grid_col;

// Create a mask to avoid redetecting existing features.
Mat mask(curr_img.rows, curr_img.cols, CV_8U, Scalar(1));

for (const auto& features : *curr_features_ptr) {
    for (const auto& feature : features.second) {
        const int y = static_cast<int>(feature.cam0_point.y);
        const int x = static_cast<int>(feature.cam0_point.x);

        int up_lim = y-2, bottom_lim = y+3,
            left_lim = x-2, right_lim = x+3;
        if (up_lim < 0) up_lim = 0;
        if (bottom_lim > curr_img.rows) bottom_lim = curr_img.rows;
        if (left_lim < 0) left_lim = 0;
        if (right_lim > curr_img.cols) right_lim = curr_img.cols;

        Range row_range(up_lim, bottom_lim);
        Range col_range(left_lim, right_lim);
        mask(row_range, col_range) = 0;
    }
}

• 현재 이미지에서 새로운 특징점을 탐지하기 전에, 기존의 특징점이 있는 위치 주변을 마스크(mask)로 지정하여 새로운 특징점 탐지를 피하도록 합니다.
• 마스크는 새로운 특징점이 기존 특징점과 중복되지 않도록 보장하기 위해 사용됩니다.
• 기존 특징점 주위 2픽셀의 여유를 두고 주변 영역을 마스킹합니다.

2. 새로운 특징점 탐지

// Detect new features.
vector<KeyPoint> new_features(0);
detector_ptr->detect(curr_img, new_features, mask);

• 새로운 특징점을 탐지하기 위해 FAST 특징점 탐지기(detector_ptr)를 사용합니다.
• 마스크가 적용된 영역을 제외한 비어 있는 부분에서만 새로운 특징점을 탐지합니다.
• 탐지된 특징점들은 new_features 벡터에 저장됩니다.

3. 그리드 기반 특징점 분할 및 필터링

// Collect the new detected features based on the grid.
// Select the ones with top response within each grid afterwards.
vector<vector<KeyPoint> > new_feature_sieve(processor_config.grid_row * processor_config.grid_col);
for (const auto& feature : new_features) {
    int row = static_cast<int>(feature.pt.y / grid_height);
    int col = static_cast<int>(feature.pt.x / grid_width);
    new_feature_sieve[row * processor_config.grid_col + col].push_back(feature);
}

new_features.clear();
for (auto& item : new_feature_sieve) {
    if (item.size() > processor_config.grid_max_feature_num) {
        std::sort(item.begin(), item.end(), &ImageProcessor::keyPointCompareByResponse);
        item.erase(item.begin() + processor_config.grid_max_feature_num, item.end());
    }
    new_features.insert(new_features.end(), item.begin(), item.end());
}

• 새로 탐지된 특징점을 그리드(grid) 기반으로 나누어 정리합니다.
  • 이미지의 각 영역에서 일정한 수의 특징점이 유지되도록 하기 위해 이미지를 그리드로 나눕니다.
• 각 그리드에 너무 많은 특징점이 탐지된 경우, 특징점 응답값이 높은 순서로 정렬하여 최대 허용 개수(grid_max_feature_num)만 남깁니다.
  • 이 과정을 통해 각 그리드에 품질이 높은 특징점이 유지되도록 합니다.

4. 스테레오 매칭

// Find the stereo matched points for the newly detected features.
vector<cv::Point2f> cam0_points(new_features.size());
for (int i = 0; i < new_features.size(); ++i)
    cam0_points[i] = new_features[i].pt;

vector<cv::Point2f> cam1_points(0);
vector<unsigned char> inlier_markers(0);
stereoMatch(cam0_points, cam1_points, inlier_markers);

vector<cv::Point2f> cam0_inliers(0);
vector<cv::Point2f> cam1_inliers(0);
vector<float> response_inliers(0);
for (int i = 0; i < inlier_markers.size(); ++i) {
    if (inlier_markers[i] == 0) continue;
    cam0_inliers.push_back(cam0_points[i]);
    cam1_inliers.push_back(cam1_points[i]);
    response_inliers.push_back(new_features[i].response);
}

• 새로 탐지된 특징점들은 스테레오 매칭을 통해 오른쪽 카메라(cam1) 이미지에서 대응점을 찾습니다.
  • 이를 통해 새롭게 추가되는 특징점이 양쪽 카메라에서 모두 관측될 수 있는 점들만 남기도록 합니다.
• inlier_markers 배열은 스테레오 매칭이 성공한 특징점에 대해 1로 표시하고, 실패한 특징점에 대해 0으로 표시합니다.
• 스테레오 매칭에 성공한 점들만 cam0_inliers와 cam1_inliers에 저장합니다.

5. 그리드에 새로운 특징점 추가

// Group the features into grids
GridFeatures grid_new_features;
for (int code = 0; code < processor_config.grid_row * processor_config.grid_col; ++code)
    grid_new_features[code] = vector<FeatureMetaData>(0);

for (int i = 0; i < cam0_inliers.size(); ++i) {
    const cv::Point2f& cam0_point = cam0_inliers[i];
    const cv::Point2f& cam1_point = cam1_inliers[i];
    const float& response = response_inliers[i];

    int row = static_cast<int>(cam0_point.y / grid_height);
    int col = static_cast<int>(cam0_point.x / grid_width);
    int code = row * processor_config.grid_col + col;

    FeatureMetaData new_feature;
    new_feature.response = response;
    new_feature.cam0_point = cam0_point;
    new_feature.cam1_point = cam1_point;
    grid_new_features[code].push_back(new_feature);
}

// Sort the new features in each grid based on its response.
for (auto& item : grid_new_features)
    std::sort(item.second.begin(), item.second.end(), &ImageProcessor::featureCompareByResponse);

int new_added_feature_num = 0;
// Collect new features within each grid with high response.
for (int code = 0; code < processor_config.grid_row * processor_config.grid_col; ++code) {
    vector<FeatureMetaData>& features_this_grid = (*curr_features_ptr)[code];
    vector<FeatureMetaData>& new_features_this_grid = grid_new_features[code];

    if (features_this_grid.size() >= processor_config.grid_min_feature_num) continue;

    int vacancy_num = processor_config.grid_min_feature_num - features_this_grid.size();
    for (int k = 0; k < vacancy_num && k < new_features_this_grid.size(); ++k) {
        features_this_grid.push_back(new_features_this_grid[k]);
        features_this_grid.back().id = next_feature_id++;
        features_this_grid.back().lifetime = 1;

        ++new_added_feature_num;
    }
}

• 새로 추가할 특징점들을 각 그리드에 할당합니다. 여기서 특징점의 응답값에 따라 각 그리드 내에서 품질이 높은 특징점을 먼저 추가하도록 합니다.
• 현재 프레임에서 특징점이 부족한 그리드에 대해서만 새로운 특징점을 추가합니다.
  • 각 그리드는 최소 grid_min_feature_num 개수만큼의 특징점을 유지하도록 하며, 빈 자리에 새로 탐지된 특징점을 추가합니다.
• 새로 추가된 특징점은 new_added_feature_num을 통해 개수를 기록합니다.

요약

addNewFeatures 함수는 다음과 같은 과정을 통해 현재 프레임에서 새로운 특징점을 추가합니다:

1. 이미지 마스크 생성: 기존의 특징점 주변을 마스킹하여 새롭게 탐지되는 특징점이 기존의 점들과 중복되지 않도록 합니다.
2. 새로운 특징점 탐지: 마스크로 보호된 영역을 제외한 부분에서 새로운 특징점을 탐지합니다.
3. 그리드 기반 필터링: 이미지를 그리드로 나누어 각 영역에 균형 잡힌 수의 특징점이 존재하도록 관리합니다.
4. 스테레오 매칭: 새로 탐지된 특징점이 오른쪽 카메라에서도 관측 가능한지 확인하여 유효한 특징점만 남깁니다.
5. 그리드에 특징점 추가: 최소한의 특징점을 유지하지 못하는 그리드에 새로 탐지된 특징점을 추가하여, 프레임 전반에 걸쳐 특징점이 고르게 분포되도록 합니다.

이 과정은 특징점의 고른 분포와 적절한 수 유지를 목표로 하며, Visual-Inertial Odometry (VIO)와 같은 시스템에서 안정적이고 일관된 특징 추적을 위해 필수적인 역할을 합니다.