File size: 2,266 Bytes
4c75661 1fc7ce0 4c75661 1fc7ce0 4c75661 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 | // 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;
}
|