vkgs-sim / tests /cpp /test_segment_map.cpp
kimjunhan's picture
vkgs-sim ์†Œ์Šค (nusim hub upload code)
1fc7ce0 verified
Raw History Blame Contribute Delete
2.27 kB
// segment_map.hpp ์‹œํ—˜ (tests/test_segment_map_cpp.py ๊ฐ€ ์ปดํŒŒ์ผยท์‹คํ–‰). ์ฐฝ ์—†์Œ.
#include <cmath>
#include <cstdio>
#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;
}