BuildVoxelMap 함수 수학적 모델링

1. 기초 개념 정의

• 객체 좌표계에서의 포인트 좌표는 pb∈R3으로 표현되며, 측정 노이즈는 평균 0 고정밀 분포를 따른다. 공분산 행렬은 Cb로 나타낸다( calcBodyCov 함수로 깊이 오차와 빔 각도 오차를 기반으로 계산).
• 객체가 세계 좌표계에서 회전 행렬 R과 이동 벡터 t로 표현된다. 세계 좌표계의 포인트 좌표는 pw=Rpb+t로 계산된다.
• 상태 추정치 R과 t에는 작은 각도 회전 벡터 δθ와 이동 오차 δt가 존재하며, 이들의 공분산 행렬은 각각 Cθ와 Ct이다. δθ는 객체 좌표계에 정의되며, 실제 회전은 Rtrue=Rexp([δθ]×)로 표현된다. 여기서 [⋅]×는 반대칭 행렬을 의미한다.

2. 오차 전파 분석

실제 세계 좌표는 다음과 같이 근사화된다: pwtrue=Rexp([δθ]×)pb+t+δt. 지수 항을 1차 근사식 exp([δθ]×)≈I+[δθ]×로 대체하면: pwtrue≈R(I+[δθ]×)pb+t+δt=Rpb+t+R[δθ]×pb+δt. 반대칭 행렬 성질을 이용해 R[δθ]×pb=-R[pb]×δθ로 변환하면, 오차는 다음과 같다: δpw=pwtrue-pw=-R[pb]×δθ+Rδpb+δt. 여기서 δpb는 측정 노이즈이며 상태 오차와 독립적이다.

3. 공분산 전달 방정식

오차 전파식을 기반으로 세계 좌표계 포인트의 공분산 행렬은 다음과 같다: Cw=E[δpwδpwT]=RCbRT+R[pb]×Cθ[pb]×TRT+Ct. (1) 첫 번째 항은 회전된 측정 노이즈의 기여, 두 번째 항은 회전 오차의 영향, 세 번째 항은 이동 오차의 기여를 나타낸다.

4. 코드 구현 분석

각 포인트 처리 과정은 다음과 같다:

• 객체 좌표계의 측정 공분산 var(=Cb) 계산.
• 객체 좌표계 포인트의 반대칭 행렬 point_crossmat(=[pb]×) 생성.
• 최종 공분산 계산:

var = (state.rot_end * extR) * var * (state.rot_end * extR).transpose() +
     (-point_crossmat) * state.cov.block<3, 3>(0, 0) * (-point_crossmat).transpose() +
     state.cov.block<3, 3>(3, 3);

5. 전체 함수 구현(핵심 주석 포함)

void VoxelMapManager::BuildVoxelMap()
{
float voxel_size = config_setting.max_voxel_size; // 범위 설정
float planar_thres = config_setting.plane_threshold; // 평면 감지 기준
int max_depth = config_setting.max_layer; // 최대 층수
int max_point_count = config_setting.max_point_num; // 최대 포인트 수
std::vector layer_init_counts = config_setting.layer_init_counts; // 층별 초기 포인트 수

std::vector input_points; // 입력 포인트 목록

for (size_t i = 0; i < feats_down_world->size(); i++) // 세계 좌표 포인트 순회
{
PointWithCov pv;
pv.world_point << feats_down_world->points[i].x, feats_down_world->points[i].y, feats_down_world->points[i].z; // 세계 좌표
V3D current_point(feats_down_body->points[i].x, feats_down_body->points[i].y, feats_down_body->points[i].z); // 객체 좌표
M3D cov_matrix; // 공분산 행렬
computeMeasurementCov(current_point, config_setting.depth_error, config_setting.beam_angle, cov_matrix); // 공분산 계산
M3D cross_matrix; // 반대칭 행렬
cross_matrix << SKEW_SYM_MATRX(current_point); // 반대칭 행렬 생성
cov_matrix = (state.rot_end * externalRotation) * cov_matrix * (state.rot_end * externalRotation).transpose() +
(-cross_matrix) * state.cov.block<3, 3>(0, 0) * (-cross_matrix).transpose() + state.cov.block<3, 3>(3, 3); // 총 공분산 계산
pv.covariance = cov_matrix; // 공분산 할당
input_points.push_back(pv); // 입력 목록 추가
}

uint point_count = input_points.size(); // 포인트 개수
for (uint i = 0; i < point_count; i++) // 입력 포인트 순회
{
const PointWithCov p = input_points[i]; // 현재 포인트
float index_coords[3]; // 인덱스 좌표
for (int j = 0; j < 3; j++)
{
index_coords[j] = p.world_point[j] / voxel_size; // 인덱스 계산
if (index_coords[j] < 0) { index_coords[j] -= 1.0; } // 음수 처리
}
VOXEL_COORDINATE pos((int64_t)index_coords[0], (int64_t)index_coords[1], (int64_t)index_coords[2]); // 체소 위치
auto iter = voxel_map.find(pos); // 체소 검색
if (iter != voxel_map.end()) // 체소 존재 시
{
iter->second->temporary_points.push_back(p); // 임시 포인트 추가
iter->second->new_point_count++; // 새로운 포인트 수 증가
}
else // 체소 미존재 시
{
VoxelOctree* octree = new VoxelOctree(max_depth, 0, layer_init_counts[0], max_point_count, planar_thres); // 오クト리 생성
voxel_map[pos] = octree; // 체소 맵에 추가
voxel_map[pos]->quarter_length = voxel_size / 4; // 사분의 일 길이 설정
voxel_map[pos]->center[0] = (0.5 + pos.x) * voxel_size; // 중심 좌표 설정
voxel_map[pos]->center[1] = (0.5 + pos.y) * voxel_size;
voxel_map[pos]->center[2] = (0.5 + pos.z) * voxel_size;
voxel_map[pos]->temporary_points.push_back(p); // 임시 포인트 추가
voxel_map[pos]->new_point_count++; // 새로운 포인트 수 증가
voxel_map[pos]->layer_init_counts = layer_init_counts; // 층 초기화 설정
}
}
for (auto& entry : voxel_map) // 체소 순회
{
entry.second->initialize_octree(); // 오クト리 초기화
}
}