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;
}