Directory: | ./ |
---|---|
File: | include/coal/serialization/octree.h |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 41 | 41 | 100.0% |
Branches: | 30 | 60 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2023-2024 INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef COAL_SERIALIZATION_OCTREE_H | ||
6 | #define COAL_SERIALIZATION_OCTREE_H | ||
7 | |||
8 | #include <sstream> | ||
9 | #include <iostream> | ||
10 | |||
11 | #include <boost/serialization/string.hpp> | ||
12 | |||
13 | #include "coal/octree.h" | ||
14 | #include "coal/serialization/fwd.h" | ||
15 | |||
16 | namespace boost { | ||
17 | namespace serialization { | ||
18 | |||
19 | namespace internal { | ||
20 | struct OcTreeAccessor : coal::OcTree { | ||
21 | typedef coal::OcTree Base; | ||
22 | using Base::default_occupancy; | ||
23 | using Base::free_threshold; | ||
24 | using Base::occupancy_threshold; | ||
25 | using Base::tree; | ||
26 | }; | ||
27 | } // namespace internal | ||
28 | |||
29 | template <class Archive> | ||
30 | 1 | void save_construct_data(Archive &ar, const coal::OcTree *octree_ptr, | |
31 | const unsigned int /*version*/) { | ||
32 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | const coal::Scalar resolution = octree_ptr->getResolution(); |
33 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ar << make_nvp("resolution", resolution); |
34 | 1 | } | |
35 | |||
36 | template <class Archive> | ||
37 | 16 | void save(Archive &ar, const coal::OcTree &octree, | |
38 | const unsigned int /*version*/) { | ||
39 | typedef internal::OcTreeAccessor Accessor; | ||
40 | 16 | const Accessor &access = reinterpret_cast<const Accessor &>(octree); | |
41 | |||
42 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | std::ostringstream stream; |
43 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | access.tree->write(stream); |
44 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | const std::string stream_str = stream.str(); |
45 | 16 | auto size = stream_str.size(); | |
46 | // We can't directly serialize stream_str because it contains binary data. | ||
47 | // This create a bug on Windows with text_archive | ||
48 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | ar << make_nvp("tree_data_size", size); |
49 |
2/4✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
|
16 | ar << make_nvp("tree_data", |
50 | make_array(stream_str.c_str(), stream_str.size())); | ||
51 | |||
52 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
|
16 | ar << make_nvp("base", base_object<coal::CollisionGeometry>(octree)); |
53 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | ar << make_nvp("default_occupancy", access.default_occupancy); |
54 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | ar << make_nvp("occupancy_threshold", access.occupancy_threshold); |
55 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | ar << make_nvp("free_threshold", access.free_threshold); |
56 | 16 | } | |
57 | |||
58 | template <class Archive> | ||
59 | 1 | void load_construct_data(Archive &ar, coal::OcTree *octree_ptr, | |
60 | const unsigned int /*version*/) { | ||
61 | coal::Scalar resolution; | ||
62 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | ar >> make_nvp("resolution", resolution); |
63 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | new (octree_ptr) coal::OcTree(resolution); |
64 | 1 | } | |
65 | |||
66 | template <class Archive> | ||
67 | 16 | void load(Archive &ar, coal::OcTree &octree, const unsigned int /*version*/) { | |
68 | typedef internal::OcTreeAccessor Accessor; | ||
69 | 16 | Accessor &access = reinterpret_cast<Accessor &>(octree); | |
70 | |||
71 | std::size_t tree_data_size; | ||
72 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | ar >> make_nvp("tree_data_size", tree_data_size); |
73 | |||
74 | 16 | std::string stream_str; | |
75 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | stream_str.resize(tree_data_size); |
76 | /// TODO use stream_str.data in C++17 | ||
77 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
|
16 | assert(tree_data_size > 0 && "tree_data_size should be greater than 0"); |
78 |
3/6✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
|
16 | ar >> make_nvp("tree_data", make_array(&stream_str[0], tree_data_size)); |
79 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | std::istringstream stream(stream_str); |
80 | |||
81 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | octomap::AbstractOcTree *new_tree = octomap::AbstractOcTree::read(stream); |
82 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
16 | access.tree = std::shared_ptr<const octomap::OcTree>( |
83 |
1/2✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
|
16 | dynamic_cast<octomap::OcTree *>(new_tree)); |
84 | |||
85 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
|
16 | ar >> make_nvp("base", base_object<coal::CollisionGeometry>(octree)); |
86 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | ar >> make_nvp("default_occupancy", access.default_occupancy); |
87 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | ar >> make_nvp("occupancy_threshold", access.occupancy_threshold); |
88 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | ar >> make_nvp("free_threshold", access.free_threshold); |
89 | 16 | } | |
90 | |||
91 | template <class Archive> | ||
92 | 32 | void serialize(Archive &ar, coal::OcTree &octree, const unsigned int version) { | |
93 | 32 | split_free(ar, octree, version); | |
94 | 32 | } | |
95 | |||
96 | } // namespace serialization | ||
97 | } // namespace boost | ||
98 | |||
99 | 36 | COAL_SERIALIZATION_DECLARE_EXPORT(::coal::OcTree) | |
100 | |||
101 | #endif // ifndef COAL_SERIALIZATION_OCTREE_H | ||
102 |