// segment_map.hpp 시험 (tests/test_segment_map_cpp.py 가 컴파일·실행). 창 없음. #include #include #include "segment_map.hpp" #define CHECK(c) \ do { \ if (!(c)) { \ std::printf("실패 %s:%d %s\n", __FILE__, __LINE__, #c); \ return 1; \ } \ } while (0) int main(int argc, char** argv) { auto m = vkgs::SegmentMap::load(argv[1]); CHECK(m.segs.size() == 3); CHECK(m.segs[1].offset[0] == 50.0 && m.segs[2].ply == "/a b/seg2/point_cloud.ply"); // 경로에 공백 CHECK(m.max_gaussians() == 300); // 구간 0 이 x 0..49, 1 이 50..99, 2 가 100..149 를 맡는다 (y = 0) CHECK(m.nearest(10, 0) == 0 && m.nearest(75, 0) == 1 && m.nearest(140, 5) == 2); // 경계(49.5) 바로 넘어서는 지금 구간을 유지, hysteresis(3 m) 의 절반 넘게 지나면 바꾼다 CHECK(m.pick(50.5, 0, 0) == 0); CHECK(m.pick(53.0, 0, 0) == 1); CHECK(m.pick(48.5, 0, 1) == 1); // 되돌아가도 경계에서 바로 바꾸지 않는다 CHECK(m.pick(46.0, 0, 1) == 0); CHECK(m.pick(120, 0, 0) == 2); // 멀리 건너뛰기 (자유 시점) // 노면 높이 (P 줄 z): 궤적 선분에 내린 발에서 선형 보간, 끝 선분은 5 m 까지 연장, 경사 = 앞바퀴 축과의 높이 차 auto near = [](float a, float b) { return std::abs(a - b) < 1e-3f; }; CHECK(m.has_z); CHECK(near(m.ground_z(2, 120.5f, 1.0f), 2.05f)); CHECK(near(m.ground_z(2, 152.0f, 0.0f), 5.2f)); // 마지막 점(149) 너머 3 m — 연장 CHECK(near(m.ground_z(2, 160.0f, 0.0f), 5.4f)); // 5 m 까지만 CHECK(near(m.ground_z(0, 10.0f, 0.0f), 0.0f)); // z 없는 줄 = 0 float z, grade; m.ground_pose(2, 110.0f, 0.0f, 0.0f, 2.9f, z, grade); CHECK(near(z, 1.0f) && near(grade, std::atan(0.1f))); m.ground_pose(2, 130.0f, 0.0f, 3.14159265f, 2.9f, z, grade); // 거꾸로 달리면 내리막 CHECK(near(z, 3.0f) && near(grade, -std::atan(0.1f))); std::printf("ok\n"); return 0; }