├── .clang-format
├── .github
└── stale.yml
├── .gitignore
├── .gitmodules
├── LICENSE
├── README.md
├── ci
└── prepare-jenkins-slave.sh
├── global_segment_map
├── CMakeLists.txt
├── include
│ └── global_segment_map
│ │ ├── common.h
│ │ ├── icp_utils.h
│ │ ├── label_block_serialization.h
│ │ ├── label_merge_integrator.h
│ │ ├── label_tsdf_integrator.h
│ │ ├── label_tsdf_map.h
│ │ ├── label_voxel.h
│ │ ├── meshing
│ │ ├── instance_color_map.h
│ │ ├── label_color_map.h
│ │ ├── label_tsdf_mesh_integrator.h
│ │ └── semantic_color_map.h
│ │ ├── segment.h
│ │ ├── semantic_instance_label_fusion.h
│ │ ├── test
│ │ └── layer_test_utils.h
│ │ └── utils
│ │ ├── file_utils.h
│ │ ├── layer_utils.h
│ │ ├── map_utils.h
│ │ ├── meshing_utils.h
│ │ └── visualizer.h
├── package.xml
└── src
│ ├── icp_utils.cc
│ ├── label_block_serialization.cc
│ ├── label_merge_integrator.cc
│ ├── label_tsdf_integrator.cc
│ ├── label_tsdf_map.cc
│ ├── meshing
│ ├── instance_color_map.cc
│ ├── label_color_map.cc
│ ├── label_tsdf_mesh_integrator.cc
│ └── semantic_color_map.cc
│ ├── segment.cc
│ ├── semantic_instance_label_fusion.cc
│ └── utils
│ └── visualizer.cc
├── global_segment_map_node
├── CMakeLists.txt
├── cfg
│ ├── asl_office_floor.yaml
│ ├── default.yaml
│ ├── orb_slam2.yaml
│ ├── royal_panda.yaml
│ ├── scenenet.yaml
│ ├── scenenn.yaml
│ └── scenenn_1hz.yaml
├── include
│ └── global_segment_map_node
│ │ ├── controller.h
│ │ └── conversions.h
├── launch
│ ├── asl_office_floor_dataset.launch
│ ├── gsm_node.launch
│ ├── orb_slam2.launch
│ ├── royal_panda.launch
│ ├── scenenn_dataset.launch
│ └── vpp_pipeline.launch
├── package.xml
└── src
│ ├── controller.cpp
│ └── node.cpp
├── voxblox-plusplus_https.rosinstall
└── voxblox-plusplus_ssh.rosinstall
/.clang-format:
--------------------------------------------------------------------------------
1 | ---
2 | Language: Cpp
3 | BasedOnStyle: Google
4 | ColumnLimit: 80
5 | PointerAlignment: Left
6 | DerivePointerAlignment: false
7 | AllowShortFunctionsOnASingleLine: Empty
8 | AllowShortIfStatementsOnASingleLine: false
9 | AllowShortLoopsOnASingleLine: false
10 | ---
11 | Language: Proto
12 | BasedOnStyle: Google
13 | ...
14 |
--------------------------------------------------------------------------------
/.github/stale.yml:
--------------------------------------------------------------------------------
1 | # Configuration for probot-stale - https://github.com/probot/stale
2 |
3 | # Number of days of inactivity before an issue becomes stale
4 | daysUntilStale: 30
5 |
6 | # Number of days of inactivity before a stale issue is closed
7 | daysUntilClose: 7
8 |
9 | # Issues with these labels will never be considered stale
10 | exemptLabels:
11 | - pinned
12 | - security
13 |
14 | # Label to use when marking an issue as stale
15 | staleLabel: "stale"
16 |
17 | # Comment to post when marking an issue as stale. Set to `false` to disable
18 | markComment: >
19 | This issue has been automatically marked as stale because it has not had
20 | recent activity. It will be closed in 7 days if no further activity occurs.
21 |
22 | # Comment to post when closing a stale issue. Set to `false` to disable
23 | closeComment: false
24 |
--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | # Compiled Object files
2 | *.slo
3 | *.lo
4 | *.o
5 | *.obj
6 |
7 | # Precompiled Headers
8 | *.gch
9 | *.pch
10 |
11 | # Compiled Dynamic libraries
12 | *.so
13 | *.dylib
14 | *.dll
15 |
16 | # Fortran module files
17 | *.mod
18 |
19 | # Compiled Static libraries
20 | *.lai
21 | *.la
22 | *.a
23 | *.lib
24 |
25 | # Executables
26 | *.exe
27 | *.out
28 | *.app
29 |
30 | # Eclipse
31 | .cproject
32 | .project
33 | /Debug/
34 | .settings
35 |
36 | # Python (for lint)
37 | *.pyc
38 |
39 | # Temporary files
40 | *~
41 |
42 | # Mac
43 | .DS_Store
44 |
45 | # Mesh results
46 | mesh_results
47 |
48 | # Tools
49 | autolintc
50 |
51 | *.ply
52 |
--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "voxblox"]
2 | path = voxblox
3 | url = https://github.com/ethz-asl/voxblox.git
4 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | BSD 3-Clause License
2 |
3 | Copyright (c) 2019, ASL, ETH Zurich, Switzerland
4 | All rights reserved.
5 |
6 | Redistribution and use in source and binary forms, with or without
7 | modification, are permitted provided that the following conditions are met:
8 |
9 | 1. Redistributions of source code must retain the above copyright notice, this
10 | list of conditions and the following disclaimer.
11 |
12 | 2. Redistributions in binary form must reproduce the above copyright notice,
13 | this list of conditions and the following disclaimer in the documentation
14 | and/or other materials provided with the distribution.
15 |
16 | 3. Neither the name of the copyright holder nor the names of its
17 | contributors may be used to endorse or promote products derived from
18 | this software without specific prior written permission.
19 |
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Voxblox++
2 |
3 | [](https://jenkins.asl.ethz.ch/job/voxblox-plusplus-nightly/label=ubuntu-xenial/)
4 | [](https://jenkins.asl.ethz.ch/job/voxblox-plusplus-nightly/label=ubuntu-bionic/)
5 |
6 | **Voxblox++** is a framework for incrementally building volumetric object-centric maps during online scanning with a localized RGB-D camera. Besides accurately describing the geometry of the reconstructed scene, the built maps contain information about the individual object instances observed in the scene. In particular, the proposed framework retrieves the dense shape and pose of recognized semantic objects, as well as of newly discovered, previously unobserved object-like instances.
7 |
8 |
9 |
11 |
12 |
13 | ## Getting started
14 | - [Installing on Ubuntu](https://github.com/ethz-asl/voxblox_gsm/wiki/Installation-on-Ubuntu)
15 | - [Sample Datasets](https://github.com/ethz-asl/voxblox-plusplus/wiki/Sample-Datasets)
16 | - [Basic usage](https://github.com/ethz-asl/voxblox-plusplus/wiki/Basic-usage)
17 | - [The Voxblox++ node](https://github.com/ethz-asl/voxblox-plusplus/wiki/The-voxblox-plusplus-node)
18 |
19 | More information and sample datasets can be found in the [wiki pages](https://github.com/ethz-asl/voxblox-plusplus/wiki).
20 |
21 | ## Citing
22 | The **Voxblox++** framework is described in the following publication:
23 |
24 | - Margarita Grinvald, Fadri Furrer, Tonci Novkovic, Jen Jen Chung, Cesar Cadena, Roland Siegwart, and Juan Nieto, **Volumetric Instance-Aware Semantic Mapping and 3D Object Discovery**, in _IEEE Robotics and Automation Letters_, July 2019. [[PDF](https://arxiv.org/abs/1903.00268)] [[Video](https://www.youtube.com/watch?v=Jvl42VJmYxg)]
25 |
26 |
27 | ```bibtex
28 | @article{grinvald2019volumetric,
29 | author={M. {Grinvald} and F. {Furrer} and T. {Novkovic} and J. J. {Chung} and C. {Cadena} and R. {Siegwart} and J. {Nieto}},
30 | journal={IEEE Robotics and Automation Letters},
31 | title={{Volumetric Instance-Aware Semantic Mapping and 3D Object Discovery}},
32 | year={2019},
33 | volume={4},
34 | number={3},
35 | pages={3037-3044},
36 | doi={10.1109/LRA.2019.2923960},
37 | ISSN={2377-3766},
38 | month={July},
39 | }
40 | ```
41 |
42 | The original geometry-only framework was introduced in the following publication:
43 |
44 | - Fadri Furrer, Tonci Novkovic, Marius Fehr, Abel Gawel, Margarita Grinvald, Torsten Sattler, Roland Siegwart, and Juan Nieto, **Incremental Object Database: Building 3D Models from Multiple Partial Observations**, in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_, October 2018. [[PDF](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8594391)] [[Video](https://www.youtube.com/watch?v=9_xg92qqw70)]
45 |
46 | ```bibtex
47 | @inproceedings{8594391,
48 | author={F. {Furrer} and T. {Novkovic} and M. {Fehr} and A. {Gawel} and M. {Grinvald} and T. {Sattler} and R. {Siegwart} and J. {Nieto}},
49 | booktitle={2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
50 | title={{Incremental Object Database: Building 3D Models from Multiple Partial Observations}},
51 | year={2018},
52 | pages={6835-6842},
53 | doi={10.1109/IROS.2018.8594391},
54 | ISSN={2153-0866},
55 | month={Oct},
56 | }
57 | ```
58 |
59 | If you use **Voxblox++** in your research, please cite accordingly.
60 |
61 | ## License
62 | The code is available under the [BSD-3-Clause license](https://github.com/ethz-asl/voxblox-plusplus/blob/master/LICENSE).
63 |
--------------------------------------------------------------------------------
/ci/prepare-jenkins-slave.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash -e
2 | echo "Running the prepare script for voxblox-plusplus.";
3 |
4 | # Install protobuf.
5 | sudo apt-get install protobuf-compiler libprotoc-dev
6 |
7 | # Set C++14.
8 | catkin config --cmake-args -DCMAKE_CXX_STANDARD=14
9 |
--------------------------------------------------------------------------------
/global_segment_map/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(global_segment_map)
3 |
4 | add_definitions(-std=c++14)
5 | if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
6 | add_definitions(-fext-numeric-literals)
7 | endif()
8 |
9 | find_package(catkin_simple REQUIRED)
10 | catkin_simple(ALL_DEPS_REQUIRED)
11 |
12 | cs_add_library(${PROJECT_NAME}
13 | src/label_block_serialization.cc
14 | src/semantic_instance_label_fusion.cc
15 | src/label_merge_integrator.cc
16 | src/icp_utils.cc
17 | src/label_tsdf_integrator.cc
18 | src/label_tsdf_map.cc
19 | src/meshing/label_tsdf_mesh_integrator.cc
20 | src/meshing/label_color_map.cc
21 | src/meshing/instance_color_map.cc
22 | src/meshing/semantic_color_map.cc
23 | src/segment.cc
24 | src/utils/visualizer.cc
25 | )
26 |
27 | cs_install()
28 | cs_export()
29 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/common.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_COMMON_H_
2 | #define GLOBAL_SEGMENT_MAP_COMMON_H_
3 |
4 | #include
5 |
6 | #include
7 | #include
8 |
9 | #include "voxblox/core/common.h"
10 |
11 | namespace voxblox {
12 |
13 | const uint16_t BackgroundLabel = 0u;
14 |
15 | // Voxblox++ custom types.
16 | typedef uint16_t Label;
17 | typedef uint16_t LabelConfidence;
18 | typedef uint16_t InstanceLabel;
19 | typedef uint8_t SemanticLabel;
20 |
21 | typedef std::vector Labels;
22 | typedef std::vector SemanticLabels;
23 | typedef std::vector InstanceLabels;
24 |
25 | typedef std::map LMap;
26 | typedef std::map::iterator LMapIt;
27 | typedef std::map LLMap;
28 | typedef std::map::iterator LLMapIt;
29 | typedef std::set LSet;
30 | typedef std::set::iterator LSetIt;
31 | typedef std::map LLSet;
32 | typedef std::map::iterator LLSetIt;
33 | typedef std::map SLMap;
34 | typedef std::map LSLMap;
35 |
36 | struct LabelCount {
37 | Label label = 0u;
38 | LabelConfidence label_confidence = 0u;
39 | };
40 |
41 | struct PointSurfelLabel {
42 | PCL_ADD_POINT4D;
43 | PCL_ADD_NORMAL4D;
44 | PCL_ADD_RGB;
45 | uint32_t label;
46 |
47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 | } EIGEN_ALIGN16;
49 |
50 | struct PointSurfelSemanticInstance {
51 | PCL_ADD_POINT4D;
52 | PCL_ADD_NORMAL4D;
53 | PCL_ADD_RGB;
54 | uint8_t instance_label;
55 | SemanticLabel semantic_label;
56 |
57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58 | } EIGEN_ALIGN16;
59 |
60 | // Map pointcloud type.
61 | struct PointTSDFLabel {
62 | PCL_ADD_POINT4D;
63 |
64 | // TSDF fields.
65 | float distance;
66 | float weight;
67 |
68 | // Semantic segmentation fields.
69 | Label segment_label;
70 | SemanticLabel semantic_class;
71 |
72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 | } EIGEN_ALIGN16;
74 |
75 | typedef pcl::PointXYZRGB PointType;
76 | typedef PointSurfelLabel PointLabelType;
77 | typedef PointSurfelSemanticInstance PointSemanticInstanceType;
78 | typedef PointTSDFLabel PointMapType;
79 |
80 | } // namespace voxblox
81 |
82 | POINT_CLOUD_REGISTER_POINT_STRUCT(
83 | voxblox::PointSurfelLabel,
84 | (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)(std::uint32_t,
85 | label, label))
86 |
87 | POINT_CLOUD_REGISTER_POINT_STRUCT(
88 | voxblox::PointSemanticInstanceType,
89 | (float, x, x)(float, y, y)(float, z, z)(float, rgb, rgb)(
90 | std::uint8_t, instance_label,
91 | instance_label)(voxblox::SemanticLabel, semantic_label, semantic_label))
92 |
93 | POINT_CLOUD_REGISTER_POINT_STRUCT(
94 | voxblox::PointTSDFLabel,
95 | (float, x, x)(float, y, y)(float, z, z)(float, distance, distance)(
96 | float, weight, weight)(voxblox::Label, segment_label,
97 | segment_label)(voxblox::SemanticLabel,
98 | semantic_class, semantic_class))
99 |
100 | #endif // GLOBAL_SEGMENT_MAP_COMMON_H_
101 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/icp_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_ICP_UTILS_H_
2 | #define GLOBAL_SEGMENT_MAP_ICP_UTILS_H_
3 |
4 | #include
5 |
6 | namespace voxblox {
7 |
8 | ICP::Config getICPConfigFromGflags();
9 |
10 | } // namespace voxblox
11 |
12 | #endif // GLOBAL_SEGMENT_MAP_ICP_UTILS_H_
13 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/label_block_serialization.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_LABEL_BLOCK_SERIALIZATION_H_
2 | #define GLOBAL_SEGMENT_MAP_LABEL_BLOCK_SERIALIZATION_H_
3 |
4 | #include
5 |
6 | #include
7 | #include
8 |
9 | #include "global_segment_map/label_voxel.h"
10 |
11 | namespace voxblox {
12 |
13 | template <>
14 | void Block::deserializeFromIntegers(
15 | const std::vector& data);
16 |
17 | template <>
18 | void Block::serializeToIntegers(std::vector* data) const;
19 |
20 | } // namespace voxblox
21 |
22 | #endif // GLOBAL_SEGMENT_MAP_LABEL_BLOCK_SERIALIZATION_H_
23 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/label_merge_integrator.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_LABEL_MERGE_INTEGRATOR_H_
2 | #define GLOBAL_SEGMENT_MAP_LABEL_MERGE_INTEGRATOR_H_
3 |
4 | #include
5 |
6 | #include "global_segment_map/label_voxel.h"
7 |
8 | namespace voxblox {
9 | template <>
10 | void mergeVoxelAIntoVoxelB(const LabelVoxel& voxel_A, LabelVoxel* voxel_B);
11 | } // namespace voxblox
12 |
13 | #endif // GLOBAL_SEGMENT_MAP_LABEL_MERGE_INTEGRATOR_H_
14 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/label_tsdf_integrator.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_LABEL_TSDF_INTEGRATOR_H_
2 | #define GLOBAL_SEGMENT_MAP_LABEL_TSDF_INTEGRATOR_H_
3 |
4 | #include
5 | #include
6 |
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 |
15 | #include "global_segment_map/common.h"
16 | #include "global_segment_map/icp_utils.h"
17 | #include "global_segment_map/label_tsdf_map.h"
18 | #include "global_segment_map/segment.h"
19 | #include "global_segment_map/semantic_instance_label_fusion.h"
20 |
21 | namespace voxblox {
22 |
23 | class LabelTsdfIntegrator : public MergedTsdfIntegrator {
24 | public:
25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 | typedef LongIndexHashMapType>::type VoxelMap;
27 | typedef VoxelMap::value_type VoxelMapElement;
28 |
29 | struct LabelTsdfConfig {
30 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31 |
32 | // Label propagation parameters.
33 | // TODO(margaritaG): maybe use a relative measure, not absolue voxel count.
34 | // Minimum number of label voxels count for label propagation.
35 | size_t min_label_voxel_count = 20u;
36 | size_t max_num_icp_updates = 15u;
37 | // Truncation distance factor for label propagation.
38 | float label_propagation_td_factor = 1.0f;
39 |
40 | // Pairwise confidence-based segment merging logic.
41 | bool enable_pairwise_confidence_merging = true;
42 | float merging_min_overlap_ratio = 0.2f;
43 | int merging_min_frame_count = 30;
44 |
45 | // Semantic instance-aware segmentation.
46 | bool enable_semantic_instance_segmentation = false;
47 |
48 | // Object database logic.
49 | // Number of frames after which the updated
50 | // objects are published.
51 | int max_segment_age = 3;
52 |
53 | // Distance-based log-normal distribution of label confidence weights.
54 | bool enable_confidence_weight_dropoff = false;
55 | float lognormal_weight_mean = 0.0f;
56 | float lognormal_weight_sigma = 1.8f;
57 | float lognormal_weight_offset = 0.7f;
58 |
59 | // ICP params.
60 | bool enable_icp = false;
61 | bool keep_track_of_icp_correction = false;
62 | };
63 |
64 | LabelTsdfIntegrator(const Config& tsdf_config,
65 | const LabelTsdfConfig& label_tsdf_config,
66 | LabelTsdfMap* map);
67 |
68 | // Label propagation.
69 | void computeSegmentLabelCandidates(
70 | Segment* segment, std::map>* candidates,
71 | std::map>* segment_merge_candidates,
72 | const std::set& assigned_labels = std::set());
73 |
74 | void decideLabelPointClouds(
75 | std::vector* segments_to_integrate,
76 | std::map>* candidates,
77 | std::map>* segment_merge_candidates);
78 |
79 | // Segment integration.
80 | void integratePointCloud(const Transformation& T_G_C,
81 | const Pointcloud& points_C, const Colors& colors,
82 | const Label& label, const bool freespace_points);
83 |
84 | // Segment merging.
85 | // Not thread safe.
86 | void mergeLabels(LLSet* merges_to_publish);
87 |
88 | // Object database.
89 | void getLabelsToPublish(
90 | std::vector* segment_labels_to_publish);
91 |
92 | Transformation getIcpRefined_T_G_C(const Transformation& T_G_C_init,
93 | const Pointcloud& point_cloud);
94 |
95 | protected:
96 | // Label propagation.
97 | // Fetch the next segment label pair which has overall
98 | // the highest voxel count.
99 | bool getNextSegmentLabelPair(
100 | const std::set& labelled_segments,
101 | std::set* assigned_labels,
102 | std::map>* candidates,
103 | std::map>* segment_merge_candidates,
104 | std::pair* segment_label_pair);
105 |
106 | Label getNextUnassignedLabel(const LabelVoxel& voxel,
107 | const std::set& assigned_labels);
108 |
109 | void checkForSegmentLabelMergeCandidate(
110 | const Label& label, const int label_points_count,
111 | const int segment_points_count,
112 | std::unordered_set* merge_candidate_labels);
113 |
114 | void increaseLabelCountForSegment(
115 | Segment* segment, const Label& label, const int segment_points_count,
116 | std::map>* candidates,
117 | std::unordered_set* merge_candidate_labels);
118 |
119 | void increasePairwiseConfidenceCount(
120 | const std::vector& merge_candidates);
121 |
122 | void updateVoxelLabelAndConfidence(LabelVoxel* label_voxel,
123 | const Label& preferred_label = 0u);
124 |
125 | void addVoxelLabelConfidence(const Label& label,
126 | const LabelConfidence& confidence,
127 | LabelVoxel* label_voxel);
128 |
129 | void increaseLabelFramesCount(const Label& label);
130 |
131 | // Increase or decrease the voxel count for a label.
132 | void changeLabelCount(const Label& label, const int count);
133 |
134 | // Will return a pointer to a voxel located at global_voxel_idx in the label
135 | // layer. Thread safe.
136 | // Takes in the last_block_idx and last_block to prevent unneeded map
137 | // lookups. If the block this voxel would be in has not been allocated, a
138 | // block in temp_label_block_map_ is created/accessed and a voxel from this
139 | // map is returned instead. Unlike the layer, accessing
140 | // temp_label_block_map_ is controlled via a mutex allowing it to grow
141 | // during integration. These temporary blocks can be merged into the layer
142 | // later by calling updateLayerWithStoredBlocks()
143 | LabelVoxel* allocateStorageAndGetLabelVoxelPtr(
144 | const GlobalIndex& global_voxel_idx, Block::Ptr* last_block,
145 | BlockIndex* last_block_idx);
146 |
147 | // NOT thread safe
148 | void updateLabelLayerWithStoredBlocks();
149 |
150 | // Updates label_voxel. Thread safe.
151 | void updateLabelVoxel(const Point& point_G, const Label& label,
152 | LabelVoxel* label_voxel,
153 | const LabelConfidence& confidence = 1u);
154 |
155 | void integrateVoxel(
156 | const Transformation& T_G_C, const Pointcloud& points_C,
157 | const Colors& colors, const Label& label, const bool enable_anti_grazing,
158 | const bool clearing_ray,
159 | const VoxelMapElement& global_voxel_idx_to_point_indices,
160 | const LongIndexHashMapType>::type& voxel_map);
161 |
162 | void integrateVoxels(
163 | const Transformation& T_G_C, const Pointcloud& points_C,
164 | const Colors& colors, const Label& label, const bool enable_anti_grazing,
165 | const bool clearing_ray,
166 | const LongIndexHashMapType>::type& voxel_map,
167 | const LongIndexHashMapType>::type& clear_map,
168 | const size_t thread_idx);
169 |
170 | void integrateRays(
171 | const Transformation& T_G_C, const Pointcloud& points_C,
172 | const Colors& colors, const Label& label, const bool enable_anti_grazing,
173 | const bool clearing_ray,
174 | const LongIndexHashMapType>::type& voxel_map,
175 | const LongIndexHashMapType>::type& clear_map);
176 |
177 | FloatingPoint computeConfidenceWeight(const FloatingPoint& distance);
178 |
179 | // Not thread safe.
180 | void swapLabels(const Label& old_label, const Label& new_label);
181 |
182 | inline void clearCurrentFrameInstanceLabels() {
183 | current_to_global_instance_map_.clear();
184 | }
185 |
186 | void resetCurrentFrameUpdatedLabelsAge();
187 |
188 | void addPairwiseConfidenceCount(const LLMapIt& label_map_it,
189 | const Label& label, const int count);
190 |
191 | void adjustPairwiseConfidenceAfterMerging(const Label& new_label,
192 | const Label& old_label);
193 |
194 | bool getNextMerge(Label* new_label, Label* old_label);
195 |
196 | inline Label getFreshLabel() {
197 | CHECK_LT(*highest_label_ptr_, std::numeric_limits::max());
198 | return ++(*highest_label_ptr_);
199 | }
200 |
201 | inline InstanceLabel getFreshInstance() {
202 | CHECK_LT(*highest_instance_ptr_,
203 | std::numeric_limits::max());
204 | return ++(*highest_instance_ptr_);
205 | }
206 |
207 | // Label layer.
208 | LabelTsdfConfig label_tsdf_config_;
209 | Layer* label_layer_;
210 |
211 | // Temporary block storage, used to hold blocks that need to be created
212 | // while integrating a new pointcloud.
213 | std::mutex temp_label_block_mutex_;
214 | Layer::BlockHashMap temp_label_block_map_;
215 |
216 | // We need to prevent simultaneous access to the voxels in the map. We
217 | // could
218 | // put a single mutex on the map or on the blocks, but as voxel updating
219 | // is
220 | // the most expensive operation in integration and most voxels are close
221 | // together, both strategies would bottleneck the system. We could make a
222 | // mutex per voxel, but this is too ram heavy as one mutex = 40 bytes.
223 | // Because of this we create an array that is indexed by the first n bits
224 | // of
225 | // the voxels hash. Assuming a uniform hash distribution, this means the
226 | // chance of two threads needing the same lock for unrelated voxels is
227 | // (num_threads / (2^n)). For 8 threads and 12 bits this gives 0.2%.
228 | ApproxHashArray<12, std::mutex, GlobalIndex, LongIndexHash> mutexes_;
229 |
230 | Label* highest_label_ptr_;
231 | LMap* label_count_map_ptr_;
232 | std::mutex updated_labels_mutex_;
233 | std::set updated_labels_;
234 |
235 | // Pairwise confidence merging.
236 | LLMap pairwise_confidence_;
237 |
238 | // ICP variables.
239 | std::shared_ptr icp_;
240 | Transformation T_Gicp_G_;
241 |
242 | // Semantic instance-aware segmentation.
243 | SemanticInstanceLabelFusion* semantic_instance_label_fusion_ptr_;
244 | InstanceLabel* highest_instance_ptr_;
245 | std::map current_to_global_instance_map_;
246 |
247 | // Object database.
248 | LMap labels_to_publish_;
249 | };
250 |
251 | } // namespace voxblox
252 |
253 | #endif // GLOBAL_SEGMENT_MAP_LABEL_TSDF_INTEGRATOR_H_
254 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/label_tsdf_map.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_LABEL_TSDF_MAP_H_
2 | #define GLOBAL_SEGMENT_MAP_LABEL_TSDF_MAP_H_
3 |
4 | #include
5 | #include
6 |
7 | #include
8 | #include
9 | #include
10 | #include
11 |
12 | #include "global_segment_map/label_voxel.h"
13 | #include "global_segment_map/semantic_instance_label_fusion.h"
14 |
15 | namespace voxblox {
16 |
17 | class LabelTsdfMap {
18 | public:
19 | typedef std::shared_ptr Ptr;
20 |
21 | typedef std::pair, Layer> LayerPair;
22 |
23 | struct Config {
24 | FloatingPoint voxel_size = 0.2;
25 | size_t voxels_per_side = 16u;
26 | };
27 |
28 | explicit LabelTsdfMap(const Config& config)
29 | : tsdf_layer_(
30 | new Layer(config.voxel_size, config.voxels_per_side)),
31 | label_layer_(
32 | new Layer(config.voxel_size, config.voxels_per_side)),
33 | config_(config),
34 | highest_label_(0u),
35 | highest_instance_(0u) {}
36 |
37 | virtual ~LabelTsdfMap() {}
38 |
39 | inline Layer* getTsdfLayerPtr() { return tsdf_layer_.get(); }
40 | inline const Layer& getTsdfLayer() const { return *tsdf_layer_; }
41 |
42 | inline Layer* getLabelLayerPtr() { return label_layer_.get(); }
43 | inline const Layer& getLabelLayer() const {
44 | return *label_layer_;
45 | }
46 |
47 | inline LMap* getLabelCountPtr() { return &label_count_map_; }
48 |
49 | inline Label* getHighestLabelPtr() { return &highest_label_; }
50 |
51 | inline InstanceLabel* getHighestInstancePtr() { return &highest_instance_; }
52 |
53 | inline SemanticInstanceLabelFusion* getSemanticInstanceLabelFusionPtr() {
54 | return &semantic_instance_label_fusion_;
55 | }
56 | inline const SemanticInstanceLabelFusion& getSemanticInstanceLabelFusion()
57 | const {
58 | return semantic_instance_label_fusion_;
59 | }
60 |
61 | inline FloatingPoint block_size() const { return tsdf_layer_->block_size(); }
62 |
63 | // Get the list of all labels
64 | // for which the voxel count is greater than 0.
65 | // NOT THREAD SAFE.
66 | Labels getLabelList();
67 |
68 | // Get the list of all instance labels
69 | // for which the voxel count is greater than 0.
70 | // NOT THREAD SAFE.
71 | InstanceLabels getInstanceList();
72 |
73 | // Get the list of semantic categories of all instances
74 | // for which the voxel count is greated than 0.
75 | // NOT THREAD SAFE.
76 | void getSemanticInstanceList(InstanceLabels* instance_labels,
77 | SemanticLabels* semantic_labels);
78 |
79 | /**
80 | * Extracts separate tsdf and label layers from the gsm, for every given
81 | * label.
82 | * @param labels of segments to extract
83 | * @param label_layers_map output map
84 | * @param labels_list_is_complete true if the gsm does not contain other
85 | * labels. false if \labels is only a subset of all labels contained by
86 | * the gsm.
87 | */
88 | void extractSegmentLayers(
89 | const Labels& labels,
90 | std::unordered_map* label_layers_map,
91 | const bool labels_list_is_complete = false);
92 |
93 | void extractInstanceLayers(
94 | const InstanceLabels& instance_labels,
95 | std::unordered_map* instance_layers_map);
96 |
97 | protected:
98 | Config config_;
99 |
100 | // The layers.
101 | Layer::Ptr tsdf_layer_;
102 | Layer::Ptr label_layer_;
103 |
104 | // Bookkeping.
105 | Label highest_label_;
106 | LMap label_count_map_;
107 | InstanceLabel highest_instance_;
108 |
109 | // Semantic instance-aware segmentation.
110 | SemanticInstanceLabelFusion semantic_instance_label_fusion_;
111 | };
112 |
113 | } // namespace voxblox
114 |
115 | #endif // GLOBAL_SEGMENT_MAP_LABEL_TSDF_MAP_H_
116 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/label_voxel.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_LABEL_VOXEL_H_
2 | #define GLOBAL_SEGMENT_MAP_LABEL_VOXEL_H_
3 |
4 | #include
5 | #include
6 |
7 | #include
8 |
9 | #include "global_segment_map/common.h"
10 |
11 | namespace voxblox {
12 |
13 | struct LabelVoxel {
14 | Label label = 0u;
15 | LabelConfidence label_confidence = 0u;
16 | LabelCount label_count[3];
17 | };
18 |
19 | namespace voxel_types {
20 | const std::string kLabel = "label";
21 | } // namespace voxel_types
22 |
23 | template <>
24 | inline std::string getVoxelType() {
25 | return voxel_types::kLabel;
26 | }
27 |
28 | } // namespace voxblox
29 |
30 | #endif // GLOBAL_SEGMENT_MAP_LABEL_VOXEL_H_
31 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/meshing/instance_color_map.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_MESHING_INSTANCE_COLOR_MAP_H_
2 | #define GLOBAL_SEGMENT_MAP_MESHING_INSTANCE_COLOR_MAP_H_
3 |
4 | #include
5 |
6 | #include "global_segment_map/common.h"
7 |
8 | namespace voxblox {
9 |
10 | class InstanceColorMap {
11 | public:
12 | void getColor(const InstanceLabel& instance_label, Color* color);
13 |
14 | protected:
15 | std::map color_map_;
16 | std::shared_timed_mutex color_map_mutex_;
17 | };
18 | } // namespace voxblox
19 |
20 | #endif // GLOBAL_SEGMENT_MAP_MESHING_INSTANCE_COLOR_MAP_H_
21 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/meshing/label_color_map.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_MESHING_LABEL_COLOR_MAP_H_
2 | #define GLOBAL_SEGMENT_MAP_MESHING_LABEL_COLOR_MAP_H_
3 |
4 | #include
5 |
6 | #include "global_segment_map/common.h"
7 |
8 | namespace voxblox {
9 |
10 | class LabelColorMap {
11 | public:
12 | void getColor(const Label& label, Color* color);
13 |
14 | protected:
15 | std::map color_map_;
16 | std::shared_timed_mutex color_map_mutex_;
17 | };
18 | } // namespace voxblox
19 |
20 | #endif
21 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/meshing/label_tsdf_mesh_integrator.h:
--------------------------------------------------------------------------------
1 | // NOTE: Code adapted from open_chisel:
2 | // github.com/personalrobotics/OpenChisel/
3 |
4 | // The MIT License (MIT) Copyright (c)
5 | // 2014 Matthew Klingensmith and Ivan Dryanovski
6 | //
7 | // Permission is hereby granted, free of charge, to any person obtaining a copy
8 | // of this software and associated documentation files (the "Software"), to deal
9 | // in the Software without restriction, including without limitation the rights
10 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11 | // copies of the Software, and to permit persons to whom the Software is
12 | // furnished to do so, subject to the following conditions:
13 | //
14 | // The above copyright notice and this permission notice shall be included in
15 | // all
16 | // copies or substantial portions of the Software.
17 | //
18 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24 | // SOFTWARE.
25 |
26 | #ifndef GLOBAL_SEGMENT_MAP_LABEL_TSDF_MESH_INTEGRATOR_H_
27 | #define GLOBAL_SEGMENT_MAP_LABEL_TSDF_MESH_INTEGRATOR_H_
28 |
29 | #include
30 | #include
31 | #include
32 | #include
33 |
34 | #include
35 | #include
36 | #include
37 |
38 | #include "global_segment_map/label_tsdf_map.h"
39 | #include "global_segment_map/label_voxel.h"
40 | #include "global_segment_map/meshing/instance_color_map.h"
41 | #include "global_segment_map/meshing/label_color_map.h"
42 | #include "global_segment_map/meshing/semantic_color_map.h"
43 | #include "global_segment_map/semantic_instance_label_fusion.h"
44 | #include "global_segment_map/utils/meshing_utils.h"
45 |
46 | namespace voxblox {
47 |
48 | class MeshLabelIntegrator : public MeshIntegrator {
49 | public:
50 | enum ColorScheme {
51 | kColor = 0,
52 | kNormals,
53 | kLabel,
54 | kLabelConfidence,
55 | kSemantic,
56 | kInstance,
57 | kMerged
58 | };
59 |
60 | struct LabelTsdfConfig {
61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62 |
63 | LabelConfidence max_confidence = 200u;
64 |
65 | // Color scheme to use for the mesh color.
66 | // By default the mesh encodes the labels.
67 | ColorScheme color_scheme = ColorScheme::kLabel;
68 | SemanticColorMap::ClassTask class_task =
69 | SemanticColorMap::ClassTask::kCoco80;
70 | };
71 |
72 | MeshLabelIntegrator(
73 | const MeshIntegratorConfig& config,
74 | const MeshLabelIntegrator::LabelTsdfConfig& label_tsdf_config,
75 | LabelTsdfMap* map, MeshLayer* mesh_layer, bool* remesh = nullptr);
76 |
77 | MeshLabelIntegrator(
78 | const MeshIntegratorConfig& config,
79 | const MeshLabelIntegrator::LabelTsdfConfig& label_tsdf_config,
80 | const LabelTsdfMap& map, MeshLayer* mesh_layer, bool* remesh = nullptr);
81 |
82 | MeshLabelIntegrator(
83 | const MeshIntegratorConfig& config,
84 | const MeshLabelIntegrator::LabelTsdfConfig& label_tsdf_config,
85 | const Layer& tsdf_layer, const Layer& label_layer,
86 | MeshLayer* mesh_layer);
87 |
88 | // Generates mesh for the tsdf layer.
89 | bool generateMesh(const bool only_mesh_updated_blocks,
90 | const bool clear_updated_flag);
91 |
92 | protected:
93 | void generateMeshBlocksFunction(const BlockIndexList& all_tsdf_blocks,
94 | const bool clear_updated_flag,
95 | ThreadSafeIndex* index_getter);
96 |
97 | InstanceLabel getInstanceLabel(const Label& label);
98 |
99 | virtual void updateMeshForBlock(const BlockIndex& block_index);
100 |
101 | void updateMeshBlockColor(Block::ConstPtr tsdf_block,
102 | Block::ConstPtr label_block,
103 | Mesh* mesh_block);
104 |
105 | void updateMeshColor(const Block& tsdf_block, Mesh* mesh);
106 |
107 | void updateMeshColor(const Block& label_block, Mesh* mesh);
108 |
109 | LabelTsdfConfig label_tsdf_config_;
110 |
111 | // Having both a const and a mutable pointer to the layer allows this
112 | // integrator to work both with a const layer (in case you don't want to
113 | // clear the updated flag) and mutable layer (in case you do want to clear
114 | // the updated flag).
115 | Layer* label_layer_mutable_ptr_;
116 | const Layer* label_layer_const_ptr_;
117 |
118 | const SemanticInstanceLabelFusion* semantic_instance_label_fusion_ptr_;
119 |
120 | bool* remesh_ptr_;
121 | // This parameter is used if no valid remesh_ptr is provided to the class at
122 | // construction time.
123 | bool remesh_ = false;
124 | std::map label_instance_map_;
125 | std::shared_timed_mutex label_instance_map_mutex_;
126 |
127 | LabelColorMap label_color_map_;
128 | SemanticColorMap semantic_color_map_;
129 | InstanceColorMap instance_color_map_;
130 | };
131 |
132 | } // namespace voxblox
133 |
134 | #endif // GLOBAL_SEGMENT_MAP_LABEL_TSDF_MESH_INTEGRATOR_H_
135 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/meshing/semantic_color_map.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_MESHING_SEMANTIC_COLOR_MAP_H_
2 | #define GLOBAL_SEGMENT_MAP_MESHING_SEMANTIC_COLOR_MAP_H_
3 |
4 | #include
5 | #include
6 |
7 | #include
8 |
9 | #include "global_segment_map/common.h"
10 |
11 | namespace voxblox {
12 |
13 | class SemanticColorMap {
14 | public:
15 | enum ClassTask { kCoco80 = 0, kNyu13 };
16 |
17 | static SemanticColorMap create(const ClassTask& class_task);
18 |
19 | SemanticColorMap(const std::vector>& color_code)
20 | : color_code_(color_code) {}
21 |
22 | void getColor(const SemanticLabel& semantic_label, Color* color) const;
23 |
24 | protected:
25 | std::vector> color_code_;
26 | };
27 |
28 | // NYUv2 13 class task color coding defined in SceneNet.
29 | class Nyu13ColorMap : public SemanticColorMap {
30 | public:
31 | Nyu13ColorMap();
32 | };
33 |
34 | // COCO 80 class task color coding using the PASCAL VOC color map.
35 | class CocoColorMap : public SemanticColorMap {
36 | public:
37 | CocoColorMap();
38 | };
39 | } // namespace voxblox
40 |
41 | #endif // GLOBAL_SEGMENT_MAP_MESHING_SEMANTIC_COLOR_MAP_H_
42 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/segment.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_SEGMENT_H_
2 | #define GLOBAL_SEGMENT_MAP_SEGMENT_H_
3 |
4 | #include
5 |
6 | namespace voxblox {
7 |
8 | class Segment {
9 | public:
10 | Segment(const pcl::PointCloud& point_cloud,
11 | const Transformation& T_G_C);
12 |
13 | Segment(const pcl::PointCloud& point_cloud,
14 | const Transformation& T_G_C);
15 |
16 | Segment(
17 | const pcl::PointCloud& point_cloud,
18 | const Transformation& T_G_C);
19 |
20 | voxblox::Transformation T_G_C_;
21 | voxblox::Pointcloud points_C_;
22 | voxblox::Colors colors_;
23 | voxblox::Label label_;
24 | voxblox::SemanticLabel semantic_label_;
25 | voxblox::InstanceLabel instance_label_;
26 | };
27 | } // namespace voxblox
28 |
29 | #endif // GLOBAL_SEGMENT_MAP_SEGMENT_H_
30 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/semantic_instance_label_fusion.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_SEMANTIC_LABEL_FUSION_H_
2 | #define GLOBAL_SEGMENT_MAP_SEMANTIC_LABEL_FUSION_H_
3 |
4 | #include
5 |
6 | #include "global_segment_map/common.h"
7 |
8 | namespace voxblox {
9 |
10 | class SemanticInstanceLabelFusion {
11 | public:
12 | void increaseLabelInstanceCount(const Label& label,
13 | const InstanceLabel& instance_label);
14 |
15 | void decreaseLabelInstanceCount(const Label& label,
16 | const InstanceLabel& instance_label);
17 |
18 | void increaseLabelFramesCount(const Label& label);
19 |
20 | InstanceLabel getInstanceLabel(
21 | const Label& label, const std::set& assigned_instances =
22 | std::set()) const;
23 |
24 | InstanceLabel getInstanceLabel(
25 | const Label& label, const float count_threshold_factor,
26 | const std::set& assigned_instances =
27 | std::set()) const;
28 |
29 | void increaseLabelClassCount(const Label& label,
30 | const SemanticLabel& semantic_label);
31 |
32 | SemanticLabel getSemanticLabel(const Label& label) const;
33 |
34 | protected:
35 | std::map> label_instance_count_;
36 | std::map label_frames_count_;
37 |
38 | // Per frame voxel count of semantic label.
39 | LSLMap label_class_count_;
40 | };
41 |
42 | } // namespace voxblox
43 |
44 | #endif // GLOBAL_SEGMENT_MAP_SEMANTIC_LABEL_FUSION_H_
45 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/test/layer_test_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_LAYER_TEST_UTILS_H_
2 | #define GLOBAL_SEGMENT_MAP_LAYER_TEST_UTILS_H_
3 |
4 | #include
5 |
6 | #include
7 |
8 | namespace voxblox {
9 | namespace test {
10 |
11 | template <>
12 | void LayerTest::CompareVoxel(const LabelVoxel& voxel_A,
13 | const LabelVoxel& voxel_B) const {
14 | CHECK_EQ(voxel_A.label, voxel_B.label);
15 | CHECK_EQ(voxel_A.label_confidence, voxel_B.label_confidence);
16 | }
17 |
18 | } // namespace test
19 | } // namespace voxblox
20 |
21 | #endif // GLOBAL_SEGMENT_MAP_LAYER_TEST_UTILS_H_
22 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/utils/file_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_UTILS_FILE_UTILS_H_
2 | #define GLOBAL_SEGMENT_MAP_UTILS_FILE_UTILS_H_
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 |
10 | #include
11 |
12 | namespace voxblox {
13 | namespace file_utils {
14 |
15 | // Creates the given path, unless the path already exists, in which case
16 | // nothing is done and 0 (success) is returned.
17 | inline int makePath(const std::string& path_in, mode_t mode) {
18 | size_t previous_slash_pos = 0u;
19 | size_t current_slash_pos;
20 | std::string dir;
21 | int make_dir_status = 0;
22 |
23 | std::string path = path_in;
24 | CHECK(!path.empty());
25 | if (path.back() != '/') {
26 | // Add a trailing '/' so we can handle everything in loop.
27 | path += '/';
28 | }
29 |
30 | while ((current_slash_pos = path.find_first_of('/', previous_slash_pos)) !=
31 | std::string::npos) {
32 | dir = path.substr(0, current_slash_pos++);
33 | previous_slash_pos = current_slash_pos;
34 | if (dir.empty()) continue; // If leading / first time is 0 length.
35 | if (dir == ".") continue;
36 |
37 | const char* c_str = dir.c_str();
38 | for (size_t i = 0u; i < strlen(c_str); ++i) {
39 | if (c_str[i] < static_cast(32) ||
40 | c_str[i] > static_cast(126)) {
41 | return -1;
42 | }
43 | }
44 |
45 | if ((make_dir_status = mkdir(c_str, mode)) && errno != EEXIST) {
46 | return make_dir_status;
47 | }
48 | }
49 | // We return -1 on error above, so just return 0 here.
50 | return 0;
51 | }
52 |
53 | } // namespace file_utils
54 | } // namespace voxblox
55 |
56 | #endif // GLOBAL_SEGMENT_MAP_UTILS_FILE_UTILS_H_
57 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/utils/layer_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_UTILS_LAYER_UTILS_H_
2 | #define GLOBAL_SEGMENT_MAP_UTILS_LAYER_UTILS_H_
3 |
4 | #include
5 |
6 | #include "global_segment_map/label_voxel.h"
7 |
8 | namespace voxblox {
9 | namespace utils {
10 |
11 | template <>
12 | inline bool isSameVoxel(const LabelVoxel& voxel_A, const LabelVoxel& voxel_B) {
13 | bool is_the_same = true;
14 |
15 | is_the_same &= voxel_A.label == voxel_B.label;
16 | is_the_same &= voxel_A.label_confidence == voxel_B.label_confidence;
17 |
18 | constexpr size_t kNumberOfLabels = 40u;
19 |
20 | for (size_t i = 0u; i < kNumberOfLabels; ++i) {
21 | is_the_same &= voxel_A.label_count[i].label == voxel_B.label_count[i].label;
22 | is_the_same &= voxel_A.label_count[i].label_confidence ==
23 | voxel_B.label_count[i].label_confidence;
24 | }
25 |
26 | return is_the_same;
27 | }
28 |
29 | } // namespace utils
30 | } // namespace voxblox
31 |
32 | #endif
33 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/utils/map_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_UTILS_MAP_UTILS_H_
2 | #define GLOBAL_SEGMENT_MAP_UTILS_MAP_UTILS_H_
3 |
4 | #include
5 | #include
6 | #include
7 |
8 | namespace voxblox {
9 |
10 | inline void createPointcloudFromMap(const LabelTsdfMap& map,
11 | pcl::PointCloud* pointcloud) {
12 | CHECK_NOTNULL(pointcloud);
13 | pointcloud->clear();
14 |
15 | const Layer& tsdf_layer = map.getTsdfLayer();
16 | const Layer& label_layer = map.getLabelLayer();
17 |
18 | const SemanticInstanceLabelFusion semantic_instance_label_fusion =
19 | map.getSemanticInstanceLabelFusion();
20 |
21 | BlockIndexList blocks;
22 | tsdf_layer.getAllAllocatedBlocks(&blocks);
23 |
24 | // Cache layer settings.
25 | size_t vps = tsdf_layer.voxels_per_side();
26 | size_t num_voxels_per_block = vps * vps * vps;
27 |
28 | // Temp variables.
29 | double intensity = 0.0;
30 | // Iterate over all blocks.
31 | for (const BlockIndex& index : blocks) {
32 | const Block& tsdf_block = tsdf_layer.getBlockByIndex(index);
33 | const Block& label_block = label_layer.getBlockByIndex(index);
34 |
35 | // Iterate over all voxels in said blocks.
36 | for (size_t linear_index = 0; linear_index < num_voxels_per_block;
37 | ++linear_index) {
38 | Point coord = tsdf_block.computeCoordinatesFromLinearIndex(linear_index);
39 |
40 | TsdfVoxel tsdf_voxel = tsdf_block.getVoxelByLinearIndex(linear_index);
41 | LabelVoxel label_voxel = label_block.getVoxelByLinearIndex(linear_index);
42 |
43 | constexpr float kMinWeight = 0.0f;
44 | constexpr float kFramesCountThresholdFactor = 0.1f;
45 |
46 | if (tsdf_voxel.weight > kMinWeight) {
47 | Label segment_label = label_voxel.label;
48 | SemanticLabel semantic_class = BackgroundLabel;
49 | InstanceLabel instance_id =
50 | semantic_instance_label_fusion.getInstanceLabel(
51 | segment_label, kFramesCountThresholdFactor);
52 |
53 | if (instance_id) {
54 | semantic_class = semantic_instance_label_fusion.getSemanticLabel(
55 | label_voxel.label);
56 | }
57 |
58 | PointMapType point;
59 |
60 | point.x = coord.x();
61 | point.y = coord.y();
62 | point.z = coord.z();
63 |
64 | point.distance = tsdf_voxel.distance;
65 | point.weight = tsdf_voxel.weight;
66 |
67 | point.segment_label = segment_label;
68 | point.semantic_class = semantic_class;
69 | pointcloud->push_back(point);
70 | }
71 | }
72 | }
73 | }
74 |
75 | } // namespace voxblox
76 |
77 | #endif // GLOBAL_SEGMENT_MAP_UTILS_MAP_UTILS_H_
78 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/utils/meshing_utils.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_UTILS_MESHING_UTILS_H_
2 | #define GLOBAL_SEGMENT_MAP_UTILS_MESHING_UTILS_H_
3 |
4 | namespace voxblox {
5 | namespace utils {
6 |
7 | inline void getColorFromNormals(const Point& normals, Color* color) {
8 | CHECK_NOTNULL(color);
9 |
10 | color->r = (normals.x() * 0.5f + 0.5f) * 255.0f;
11 | color->g = (normals.y() * 0.5f + 0.5f) * 255.0f;
12 | color->b = (normals.x() * 0.5f + 0.5f) * 255.0f;
13 | color->a = 255.0f;
14 | }
15 |
16 | inline void getColorFromLabelConfidence(const LabelVoxel& label_voxel,
17 | const LabelConfidence& max_confidence,
18 | Color* color) {
19 | CHECK_NOTNULL(color);
20 | *color = rainbowColorMap(label_voxel.label_confidence / max_confidence);
21 | }
22 |
23 | } // namespace utils
24 | } // namespace voxblox
25 |
26 | #endif
27 |
--------------------------------------------------------------------------------
/global_segment_map/include/global_segment_map/utils/visualizer.h:
--------------------------------------------------------------------------------
1 | #ifndef GLOBAL_SEGMENT_MAP_UTILS_VISUALIZER_H_
2 | #define GLOBAL_SEGMENT_MAP_UTILS_VISUALIZER_H_
3 |
4 | #include
5 |
6 | #include
7 | #include
8 |
9 | namespace voxblox {
10 |
11 | class Visualizer {
12 | public:
13 | Visualizer(const std::vector>& mesh_layers,
14 | bool* mesh_layer_updated, std::mutex* mesh_layer_mutex_ptr,
15 | std::vector camera_distances,
16 | std::vector clip_distances, bool save_visualizer_frames);
17 |
18 | void visualizeMesh();
19 |
20 | std::vector> mesh_layers_;
21 |
22 | std::mutex* mesh_layer_mutex_ptr_;
23 | bool* mesh_layer_updated_;
24 |
25 | size_t frame_count_;
26 |
27 | std::vector camera_position_;
28 | std::vector clip_distances_;
29 |
30 | bool save_visualizer_frames_;
31 | };
32 | } // namespace voxblox
33 |
34 | #endif // GLOBAL_SEGMENT_MAP_UTILS_VISUALIZER_H_
35 |
--------------------------------------------------------------------------------
/global_segment_map/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | global_segment_map
4 | 0.0.0
5 | Voxblox++
6 |
7 | Margarita Grinvald
8 | Fadri Furrer
9 | Tonci Novkovic
10 | Marius Fehr
11 |
12 | Margarita Grinvald
13 | Fadri Furrer
14 | Tonci Novkovic
15 | Marius Fehr
16 |
17 | BSD
18 |
19 | catkin
20 | catkin_simple
21 |
22 | eigen_catkin
23 | glog_catkin
24 | pcl_catkin
25 | voxblox
26 |
27 |
--------------------------------------------------------------------------------
/global_segment_map/src/icp_utils.cc:
--------------------------------------------------------------------------------
1 | #include "global_segment_map/icp_utils.h"
2 |
3 | #include
4 |
5 | DEFINE_bool(icp_refine_roll_pitch, false,
6 | "If enabled, ICP will refine not only translation and yaw, but "
7 | "also roll and pitch.");
8 | DEFINE_int32(
9 | icp_mini_batch_size, 20,
10 | "Number of points used in each alignment step. To allow simple threading "
11 | "the ICP process is split up into a large number of separate alignments "
12 | "performed on small pointclouds. This parameter dictates how many points "
13 | "are used in each 'mini batch'. The result are then combined weighting "
14 | "them by an estimate of the information gained by the alignment.");
15 | DEFINE_double(icp_min_match_ratio, 0.8,
16 | "Ratio of points that must lie within the truncation distance of "
17 | "an allocated voxel");
18 | DEFINE_double(icp_subsample_keep_ratio, 0.5,
19 | "Ratio of points used in the ICP matching");
20 | DEFINE_double(
21 | icp_inital_translation_weighting, 100,
22 | "Weighting applied to the translational component of the initial guess. "
23 | "Very roughly corresponds to the inverse covariance of the initial guess "
24 | "multiplied by the variance in a measured points accuracy.");
25 | DEFINE_double(icp_inital_rotation_weighting, 100,
26 | "Weighting applied to the rotational component of the initial "
27 | "guess. See inital_translation_weighting for further details");
28 |
29 | namespace voxblox {
30 | ICP::Config getICPConfigFromGflags() {
31 | ICP::Config icp_config;
32 |
33 | icp_config.refine_roll_pitch = FLAGS_icp_refine_roll_pitch;
34 | icp_config.mini_batch_size = FLAGS_icp_mini_batch_size;
35 | icp_config.min_match_ratio = FLAGS_icp_min_match_ratio;
36 | icp_config.subsample_keep_ratio = FLAGS_icp_subsample_keep_ratio;
37 | icp_config.inital_translation_weighting =
38 | FLAGS_icp_inital_translation_weighting;
39 | icp_config.inital_rotation_weighting = FLAGS_icp_inital_rotation_weighting;
40 |
41 | return icp_config;
42 | }
43 |
44 | } // namespace voxblox
45 |
--------------------------------------------------------------------------------
/global_segment_map/src/label_block_serialization.cc:
--------------------------------------------------------------------------------
1 | #include
2 |
3 | #include
4 |
5 | #include "global_segment_map/label_block_serialization.h"
6 |
7 | namespace voxblox {
8 |
9 | template <>
10 | void Block::deserializeFromIntegers(
11 | const std::vector& data) {
12 | constexpr size_t kNumDataPacketsPerVoxel = 2u;
13 | const size_t num_data_packets = data.size();
14 | CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, num_data_packets);
15 | for (size_t voxel_idx = 0u, data_idx = 0u;
16 | voxel_idx < num_voxels_ && data_idx < num_data_packets;
17 | ++voxel_idx, data_idx += kNumDataPacketsPerVoxel) {
18 | const uint32_t bytes_1 = data[data_idx];
19 | const uint32_t bytes_2 = data[data_idx + 1u];
20 |
21 | LabelVoxel& voxel = voxels_[voxel_idx];
22 |
23 | memcpy(&(voxel.label_confidence), &bytes_1, sizeof(bytes_1));
24 | memcpy(&(voxel.label), &bytes_2, sizeof(bytes_2));
25 | }
26 | }
27 |
28 | // TODO(grinvalm): serialize and deserialize also votes array.
29 | template <>
30 | void Block::serializeToIntegers(std::vector* data) const {
31 | CHECK_NOTNULL(data);
32 | constexpr size_t kNumDataPacketsPerVoxel = 2u;
33 | data->clear();
34 | data->reserve(num_voxels_ * kNumDataPacketsPerVoxel);
35 | for (size_t voxel_idx = 0u; voxel_idx < num_voxels_; ++voxel_idx) {
36 | const LabelVoxel& voxel = voxels_[voxel_idx];
37 |
38 | const uint32_t* bytes_1_ptr =
39 | reinterpret_cast(&voxel.label_confidence);
40 | data->push_back(*bytes_1_ptr);
41 |
42 | const uint32_t* bytes_2_ptr =
43 | reinterpret_cast(&voxel.label);
44 | data->push_back(*bytes_2_ptr);
45 | }
46 | CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, data->size());
47 | }
48 |
49 | } // namespace voxblox
50 |
--------------------------------------------------------------------------------
/global_segment_map/src/label_merge_integrator.cc:
--------------------------------------------------------------------------------
1 | #include "global_segment_map/label_merge_integrator.h"
2 | #include "global_segment_map/label_voxel.h"
3 |
4 | namespace voxblox {
5 | template <>
6 | void mergeVoxelAIntoVoxelB(const LabelVoxel& voxel_A, LabelVoxel* voxel_B) {
7 | CHECK_NOTNULL(voxel_B);
8 |
9 | if (voxel_A.label == voxel_B->label) {
10 | voxel_B->label_confidence += voxel_A.label_confidence;
11 | } else if (voxel_A.label_confidence > voxel_B->label_confidence) {
12 | voxel_B->label = voxel_A.label;
13 | voxel_B->label_confidence =
14 | voxel_A.label_confidence - voxel_B->label_confidence;
15 | } else {
16 | voxel_B->label_confidence -= voxel_A.label_confidence;
17 | }
18 | }
19 | } // namespace voxblox
20 |
--------------------------------------------------------------------------------
/global_segment_map/src/label_tsdf_integrator.cc:
--------------------------------------------------------------------------------
1 | #include "global_segment_map/label_tsdf_integrator.h"
2 |
3 | namespace voxblox {
4 |
5 | LabelTsdfIntegrator::LabelTsdfIntegrator(
6 | const Config& tsdf_config, const LabelTsdfConfig& label_tsdf_config,
7 | LabelTsdfMap* map)
8 | : MergedTsdfIntegrator(tsdf_config, CHECK_NOTNULL(map->getTsdfLayerPtr())),
9 | label_tsdf_config_(label_tsdf_config),
10 | label_layer_(CHECK_NOTNULL(map->getLabelLayerPtr())),
11 | label_count_map_ptr_(map->getLabelCountPtr()),
12 | highest_label_ptr_(CHECK_NOTNULL(map->getHighestLabelPtr())),
13 | highest_instance_ptr_(CHECK_NOTNULL(map->getHighestInstancePtr())),
14 | semantic_instance_label_fusion_ptr_(
15 | map->getSemanticInstanceLabelFusionPtr()) {}
16 |
17 | void LabelTsdfIntegrator::checkForSegmentLabelMergeCandidate(
18 | const Label& label, const int label_points_count,
19 | const int segment_points_count,
20 | std::unordered_set* merge_candidate_labels) {
21 | CHECK_NOTNULL(merge_candidate_labels);
22 | // All segment labels that overlap with more than a certain
23 | // percentage of the segment points are potential merge candidates.
24 | float label_segment_overlap_ratio = static_cast(label_points_count) /
25 | static_cast(segment_points_count);
26 | if (label_segment_overlap_ratio >
27 | label_tsdf_config_.merging_min_overlap_ratio) {
28 | merge_candidate_labels->insert(label);
29 | }
30 | }
31 |
32 | void LabelTsdfIntegrator::increaseLabelCountForSegment(
33 | Segment* segment, const Label& label, const int segment_points_count,
34 | std::map>* candidates,
35 | std::unordered_set* merge_candidate_labels) {
36 | CHECK_NOTNULL(segment);
37 | CHECK_NOTNULL(candidates);
38 | CHECK_NOTNULL(merge_candidate_labels);
39 | auto label_it = candidates->find(label);
40 | if (label_it != candidates->end()) {
41 | auto segment_it = label_it->second.find(segment);
42 | if (segment_it != label_it->second.end()) {
43 | ++segment_it->second;
44 |
45 | if (label_tsdf_config_.enable_pairwise_confidence_merging) {
46 | checkForSegmentLabelMergeCandidate(label, segment_it->second,
47 | segment_points_count,
48 | merge_candidate_labels);
49 | }
50 | } else {
51 | label_it->second.emplace(segment, 1u);
52 | }
53 | } else {
54 | std::map segment_points_count;
55 | segment_points_count.emplace(segment, 1u);
56 | candidates->emplace(label, segment_points_count);
57 | }
58 | }
59 |
60 | void LabelTsdfIntegrator::increasePairwiseConfidenceCount(
61 | const std::vector& merge_candidates) {
62 | for (size_t i = 0u; i < merge_candidates.size(); ++i) {
63 | for (size_t j = i + 1; j < merge_candidates.size(); ++j) {
64 | Label new_label = merge_candidates[i];
65 | Label old_label = merge_candidates[j];
66 |
67 | if (new_label != old_label) {
68 | // Pairs consist of (new_label, old_label) where new_label <
69 | // old_label.
70 | if (new_label > old_label) {
71 | Label tmp = old_label;
72 | old_label = new_label;
73 | new_label = tmp;
74 | }
75 | // For every pair of labels from the merge candidates
76 | // set or increase their pairwise confidence.
77 | LLMapIt new_label_it = pairwise_confidence_.find(new_label);
78 | if (new_label_it != pairwise_confidence_.end()) {
79 | LMapIt old_label_it = new_label_it->second.find(old_label);
80 | if (old_label_it != new_label_it->second.end()) {
81 | ++old_label_it->second;
82 | } else {
83 | new_label_it->second.emplace(old_label, 1);
84 | }
85 | } else {
86 | LMap confidence_pair;
87 | confidence_pair.emplace(old_label, 1);
88 | pairwise_confidence_.emplace(new_label, confidence_pair);
89 | }
90 | }
91 | }
92 | }
93 | }
94 |
95 | Label LabelTsdfIntegrator::getNextUnassignedLabel(
96 | const LabelVoxel& voxel, const std::set& assigned_labels) {
97 | Label voxel_label = 0u;
98 | auto label_it = assigned_labels.find(voxel.label);
99 | if (label_it != assigned_labels.end()) {
100 | // The voxel label has been assigned already, so find
101 | // the next unassigned label with highest confidence for this voxel.
102 | LabelConfidence max_confidence = 0u;
103 | // TODO(grinvalm) in case of two or more labels
104 | // having the same confidence choose according to some logic,
105 | // not randomly.
106 | // Ideally one should avoid labels that belong to another segment
107 | // but not assigned in the current frame.
108 | Label preferred_label = 0u;
109 | for (const LabelCount& label_count : voxel.label_count) {
110 | label_it = assigned_labels.find(label_count.label);
111 | if (label_it == assigned_labels.end() &&
112 | (label_count.label_confidence >= max_confidence ||
113 | (label_count.label == preferred_label && preferred_label != 0u &&
114 | label_count.label_confidence == max_confidence))) {
115 | max_confidence = label_count.label_confidence;
116 | voxel_label = label_count.label;
117 | }
118 | }
119 | } else {
120 | // The voxel label hasn't been assigned yet, so it is valid.
121 | voxel_label = voxel.label;
122 | }
123 | return voxel_label;
124 | }
125 |
126 | void LabelTsdfIntegrator::updateVoxelLabelAndConfidence(
127 | LabelVoxel* label_voxel, const Label& preferred_label) {
128 | CHECK_NOTNULL(label_voxel);
129 | Label max_label = 0u;
130 | LabelConfidence max_confidence = 0u;
131 | for (const LabelCount& label_count : label_voxel->label_count) {
132 | if (label_count.label_confidence > max_confidence ||
133 | (label_count.label == preferred_label && preferred_label != 0u &&
134 | label_count.label_confidence == max_confidence)) {
135 | max_confidence = label_count.label_confidence;
136 | max_label = label_count.label;
137 | }
138 | }
139 | label_voxel->label = max_label;
140 | label_voxel->label_confidence = max_confidence;
141 | }
142 |
143 | void LabelTsdfIntegrator::addVoxelLabelConfidence(
144 | const Label& label, const LabelConfidence& confidence,
145 | LabelVoxel* label_voxel) {
146 | CHECK_NOTNULL(label_voxel);
147 | bool updated = false;
148 | for (LabelCount& label_count : label_voxel->label_count) {
149 | if (label_count.label == label) {
150 | // Label already observed in this voxel.
151 | label_count.label_confidence = label_count.label_confidence + confidence;
152 | updated = true;
153 | break;
154 | }
155 | }
156 | if (updated == false) {
157 | for (LabelCount& label_count : label_voxel->label_count) {
158 | if (label_count.label == 0u) {
159 | // This is the first allocated but unused index in the map
160 | // in which the new entry should be added.
161 | label_count.label = label;
162 | label_count.label_confidence = confidence;
163 | updated = true;
164 | break;
165 | }
166 | }
167 | }
168 | if (updated == false) {
169 | // TODO(margaritaG): handle this nicely or remove.
170 | // LOG(FATAL) << "Out-of-memory for storing labels and confidences for this
171 | // "
172 | // " voxel. Please increse size of array.";
173 | }
174 | }
175 |
176 | void LabelTsdfIntegrator::computeSegmentLabelCandidates(
177 | Segment* segment, std::map>* candidates,
178 | std::map>* segment_merge_candidates,
179 | const std::set& assigned_labels) {
180 | CHECK_NOTNULL(segment);
181 | CHECK_NOTNULL(candidates);
182 | CHECK_NOTNULL(segment_merge_candidates);
183 | // Flag to check whether there exists at least one label candidate.
184 | bool candidate_label_exists = false;
185 | const int segment_points_count = segment->points_C_.size();
186 | std::unordered_set merge_candidate_labels;
187 |
188 | for (const Point& point_C : segment->points_C_) {
189 | const Point point_G = segment->T_G_C_ * point_C;
190 |
191 | // Get the corresponding voxel by 3D position in world frame.
192 | Layer::BlockType::ConstPtr label_block_ptr =
193 | label_layer_->getBlockPtrByCoordinates(point_G);
194 | // Get the corresponding voxel by 3D position in world frame.
195 | Layer::BlockType::ConstPtr tsdf_block_ptr =
196 | layer_->getBlockPtrByCoordinates(point_G);
197 |
198 | if (label_block_ptr != nullptr) {
199 | const LabelVoxel& label_voxel =
200 | label_block_ptr->getVoxelByCoordinates(point_G);
201 | const TsdfVoxel& tsdf_voxel =
202 | tsdf_block_ptr->getVoxelByCoordinates(point_G);
203 | Label label = 0u;
204 | label = getNextUnassignedLabel(label_voxel, assigned_labels);
205 | if (label != 0u &&
206 | std::abs(tsdf_voxel.distance) <
207 | label_tsdf_config_.label_propagation_td_factor * voxel_size_) {
208 | // Do not consider allocated but unobserved voxels
209 | // which have label == 0.
210 | candidate_label_exists = true;
211 | increaseLabelCountForSegment(segment, label, segment_points_count,
212 | candidates, &merge_candidate_labels);
213 | }
214 | }
215 | }
216 |
217 | if (label_tsdf_config_.enable_pairwise_confidence_merging) {
218 | std::vector merge_candidates;
219 | std::copy(merge_candidate_labels.begin(), merge_candidate_labels.end(),
220 | std::back_inserter(merge_candidates));
221 | (*segment_merge_candidates)[segment] = merge_candidates;
222 | }
223 |
224 | // Previously unobserved segment gets an unseen label.
225 | if (!candidate_label_exists) {
226 | Label fresh_label = getFreshLabel();
227 | std::map map;
228 | map.insert(std::pair(segment, segment->points_C_.size()));
229 | candidates->insert(
230 | std::pair>(fresh_label, map));
231 | }
232 | }
233 |
234 | bool LabelTsdfIntegrator::getNextSegmentLabelPair(
235 | const std::set& labelled_segments,
236 | std::set* assigned_labels,
237 | std::map>* candidates,
238 | std::map>* segment_merge_candidates,
239 | std::pair* segment_label_pair) {
240 | CHECK_NOTNULL(assigned_labels);
241 | CHECK_NOTNULL(candidates);
242 | CHECK_NOTNULL(segment_merge_candidates);
243 | CHECK_NOTNULL(segment_label_pair);
244 |
245 | Label max_label;
246 | size_t max_count = 0u;
247 | Segment* max_segment;
248 | std::map segments_to_recompute;
249 |
250 | for (auto label_it = candidates->begin(); label_it != candidates->end();
251 | ++label_it) {
252 | for (auto segment_it = label_it->second.begin();
253 | segment_it != label_it->second.end(); segment_it++) {
254 | bool count_greater_than_max = segment_it->second > max_count;
255 | bool count_greater_than_min =
256 | segment_it->second > label_tsdf_config_.min_label_voxel_count;
257 | bool is_unlabelled =
258 | labelled_segments.find(segment_it->first) == labelled_segments.end();
259 | if (count_greater_than_max && count_greater_than_min && is_unlabelled) {
260 | max_count = segment_it->second;
261 | max_segment = segment_it->first;
262 | max_label = label_it->first;
263 | segments_to_recompute = label_it->second;
264 | }
265 | }
266 | }
267 | if (max_count == 0u) {
268 | return false;
269 | }
270 |
271 | segment_label_pair->first = max_segment;
272 | segment_label_pair->second = max_label;
273 | assigned_labels->emplace(max_label);
274 |
275 | // For all segments that need to have their label
276 | // count recomputed, first clean their relative entries and recompute.
277 | for (auto segment : segments_to_recompute) {
278 | if (segment.first != max_segment) {
279 | for (auto label_it = candidates->begin(); label_it != candidates->end();
280 | ++label_it) {
281 | if (label_it->first != max_label) {
282 | label_it->second.erase(segment.first);
283 | }
284 | }
285 | computeSegmentLabelCandidates(segment.first, candidates,
286 | segment_merge_candidates, *assigned_labels);
287 | }
288 | }
289 | return true;
290 | }
291 | void LabelTsdfIntegrator::decideLabelPointClouds(
292 | std::vector* segments_to_integrate,
293 | std::map>* candidates,
294 | std::map>* segment_merge_candidates) {
295 | CHECK_NOTNULL(segments_to_integrate);
296 | CHECK_NOTNULL(candidates);
297 | CHECK_NOTNULL(segment_merge_candidates);
298 | std::set assigned_labels;
299 | std::set labelled_segments;
300 | std::pair pair;
301 | std::set assigned_instances;
302 |
303 | while (getNextSegmentLabelPair(labelled_segments, &assigned_labels,
304 | candidates, segment_merge_candidates, &pair)) {
305 | Segment* segment = pair.first;
306 | CHECK_NOTNULL(segment);
307 | Label& label = pair.second;
308 |
309 | segment->label_ = label;
310 | labelled_segments.insert(segment);
311 | candidates->erase(label);
312 | }
313 |
314 | for (auto merge_candidates : *segment_merge_candidates) {
315 | increasePairwiseConfidenceCount(merge_candidates.second);
316 | }
317 |
318 | // For every segment that didn't get a label because
319 | // its label counts were too few, assign it an unseen label.
320 | for (auto segment_it = segments_to_integrate->begin();
321 | segment_it != segments_to_integrate->end(); ++segment_it) {
322 | if (labelled_segments.find(*segment_it) == labelled_segments.end()) {
323 | Label fresh = getFreshLabel();
324 | (*segment_it)->label_ = fresh;
325 | labelled_segments.insert(*segment_it);
326 | }
327 | }
328 |
329 | if (label_tsdf_config_.enable_semantic_instance_segmentation) {
330 | // Instance stuff.
331 | for (auto segment_it = labelled_segments.begin();
332 | segment_it != labelled_segments.end(); ++segment_it) {
333 | Label label = (*segment_it)->label_;
334 | if ((*segment_it)->points_C_.size() > 0u) {
335 | semantic_instance_label_fusion_ptr_->increaseLabelFramesCount(label);
336 | }
337 |
338 | // Loop through all the segments.
339 | if ((*segment_it)->instance_label_ != 0u) {
340 | // It's a segment with a current frame instance.
341 | auto global_instance_it = current_to_global_instance_map_.find(
342 | (*segment_it)->instance_label_);
343 | if (global_instance_it != current_to_global_instance_map_.end()) {
344 | // If current frame instance maps to a global instance, use it.
345 | semantic_instance_label_fusion_ptr_->increaseLabelInstanceCount(
346 | label, global_instance_it->second);
347 | } else {
348 | // Current frame instance doesn't map to any global instance.
349 | // Get the global instance with max count.
350 | InstanceLabel instance_label =
351 | semantic_instance_label_fusion_ptr_->getInstanceLabel(
352 | label, assigned_instances);
353 |
354 | if (instance_label != 0u) {
355 | current_to_global_instance_map_.emplace(
356 | (*segment_it)->instance_label_, instance_label);
357 | semantic_instance_label_fusion_ptr_->increaseLabelInstanceCount(
358 | label, instance_label);
359 | assigned_instances.emplace(instance_label);
360 | } else {
361 | // Create new global instance.
362 | InstanceLabel fresh_instance = getFreshInstance();
363 | current_to_global_instance_map_.emplace(
364 | (*segment_it)->instance_label_, fresh_instance);
365 | semantic_instance_label_fusion_ptr_->increaseLabelInstanceCount(
366 | label, fresh_instance);
367 | }
368 | }
369 | semantic_instance_label_fusion_ptr_->increaseLabelClassCount(
370 | label, (*segment_it)->semantic_label_);
371 | } else {
372 | // It's a segment with no instance prediction in the current frame.
373 | // Get the global instance it maps to, and set it as assigned.
374 | InstanceLabel instance_label =
375 | semantic_instance_label_fusion_ptr_->getInstanceLabel(label);
376 | // TODO(grinvalm) : also pass assigned instances here?
377 | if (instance_label != 0u) {
378 | assigned_instances.emplace(instance_label);
379 | }
380 | // TODO(margaritaG): handle this nicely or remove.
381 | // if ((*segment_it)->points_C_.size() > 2500) {
382 | // decreaseLabelInstanceCount(label, instance_label);
383 | // }
384 | }
385 | }
386 | }
387 | }
388 |
389 | // Increase or decrease the voxel count for a label.
390 | void LabelTsdfIntegrator::changeLabelCount(const Label& label,
391 | const int count) {
392 | auto label_count_it = label_count_map_ptr_->find(label);
393 | if (label_count_it != label_count_map_ptr_->end()) {
394 | label_count_it->second = label_count_it->second + count;
395 | if (label_count_it->second <= 0) {
396 | label_count_map_ptr_->erase(label_count_it);
397 | }
398 | } else {
399 | if (label != 0u) {
400 | // TODO(margaritaG) : This seems to not hold true occasionally,
401 | // investigate.
402 | // CHECK_GE(count, 0);
403 | label_count_map_ptr_->insert(std::make_pair(label, count));
404 | }
405 | }
406 | }
407 |
408 | LabelVoxel* LabelTsdfIntegrator::allocateStorageAndGetLabelVoxelPtr(
409 | const GlobalIndex& global_voxel_idx, Block::Ptr* last_block,
410 | BlockIndex* last_block_idx) {
411 | CHECK_NOTNULL(last_block);
412 | CHECK_NOTNULL(last_block_idx);
413 |
414 | const BlockIndex block_idx =
415 | getBlockIndexFromGlobalVoxelIndex(global_voxel_idx, voxels_per_side_inv_);
416 |
417 | // TODO(margaritaG): citing Marius: this logic makes sure that if you already
418 | // have the right block pointer you don't need to go looking for it again, so
419 | // in order to make this logic effective you need to move the block ptr and
420 | // block idx outside of the while loop in the function calling this.
421 | if ((block_idx != *last_block_idx) || (*last_block == nullptr)) {
422 | *last_block = label_layer_->getBlockPtrByIndex(block_idx);
423 | *last_block_idx = block_idx;
424 | }
425 |
426 | // If no block at this location currently exists, we allocate a temporary
427 | // voxel that will be merged into the map later
428 | if (*last_block == nullptr) {
429 | // To allow temp_label_block_map_ to grow we can only let
430 | // one thread in at once
431 | std::lock_guard lock(temp_label_block_mutex_);
432 |
433 | typename Layer::BlockHashMap::iterator it =
434 | temp_label_block_map_.find(block_idx);
435 | if (it != temp_label_block_map_.end()) {
436 | *last_block = it->second;
437 | } else {
438 | auto insert_status = temp_label_block_map_.emplace(
439 | block_idx, std::make_shared>(
440 | voxels_per_side_, voxel_size_,
441 | getOriginPointFromGridIndex(block_idx, block_size_)));
442 |
443 | CHECK(insert_status.second) << "Block already exists when allocating at "
444 | << block_idx.transpose();
445 |
446 | *last_block = insert_status.first->second;
447 | }
448 | }
449 |
450 | const VoxelIndex local_voxel_idx =
451 | getLocalFromGlobalVoxelIndex(global_voxel_idx, voxels_per_side_);
452 |
453 | return &((*last_block)->getVoxelByVoxelIndex(local_voxel_idx));
454 | }
455 |
456 | void LabelTsdfIntegrator::updateLabelLayerWithStoredBlocks() {
457 | BlockIndex last_block_idx;
458 | Block::Ptr block = nullptr;
459 |
460 | for (const std::pair::Ptr>&
461 | temp_label_block_pair : temp_label_block_map_) {
462 | label_layer_->insertBlock(temp_label_block_pair);
463 | }
464 |
465 | temp_label_block_map_.clear();
466 | }
467 |
468 | // Updates label_voxel. Thread safe.
469 | void LabelTsdfIntegrator::updateLabelVoxel(const Point& point_G,
470 | const Label& label,
471 | LabelVoxel* label_voxel,
472 | const LabelConfidence& confidence) {
473 | CHECK_NOTNULL(label_voxel);
474 | // Lookup the mutex that is responsible for this voxel and lock it.
475 | std::lock_guard lock(mutexes_.get(
476 | getGridIndexFromPoint(point_G, voxel_size_inv_)));
477 |
478 | CHECK_NOTNULL(label_voxel);
479 |
480 | // label_voxel->semantic_label = semantic_label;
481 | Label previous_label = label_voxel->label;
482 | addVoxelLabelConfidence(label, confidence, label_voxel);
483 | updateVoxelLabelAndConfidence(label_voxel, label);
484 | Label new_label = label_voxel->label;
485 |
486 | // This old semantic stuff per voxel was not thread safe.
487 | // Now all is good.
488 | // increaseLabelClassCount(new_label, semantic_label);
489 |
490 | if (new_label != previous_label) {
491 | // Both of the segments corresponding to the two labels are
492 | // updated, one gains a voxel, one loses a voxel.
493 | std::lock_guard lock(updated_labels_mutex_);
494 |
495 | updated_labels_.insert(new_label);
496 | changeLabelCount(new_label, 1);
497 |
498 | if (previous_label != 0u) {
499 | updated_labels_.insert(previous_label);
500 | changeLabelCount(previous_label, -1);
501 | }
502 |
503 | if (*highest_label_ptr_ < new_label) {
504 | *highest_label_ptr_ = new_label;
505 | }
506 | }
507 | }
508 |
509 | void LabelTsdfIntegrator::integratePointCloud(const Transformation& T_G_C,
510 | const Pointcloud& points_C,
511 | const Colors& colors,
512 | const Label& label,
513 | const bool freespace_points) {
514 | CHECK_EQ(points_C.size(), colors.size());
515 | CHECK_GE(points_C.size(), 0u);
516 |
517 | // Pre-compute a list of unique voxels to end on.
518 | // Create a hashmap: VOXEL INDEX -> index in original cloud.
519 | LongIndexHashMapType>::type voxel_map;
520 | // This is a hash map (same as above) to all the indices that need to be
521 | // cleared.
522 | LongIndexHashMapType>::type clear_map;
523 |
524 | std::unique_ptr index_getter(
525 | ThreadSafeIndexFactory::get(config_.integration_order_mode, points_C));
526 |
527 | bundleRays(T_G_C, points_C, freespace_points, index_getter.get(), &voxel_map,
528 | &clear_map);
529 |
530 | integrateRays(T_G_C, points_C, colors, label, config_.enable_anti_grazing,
531 | false, voxel_map, clear_map);
532 |
533 | integrateRays(T_G_C, points_C, colors, label, config_.enable_anti_grazing,
534 | true, voxel_map, clear_map);
535 | }
536 |
537 | void LabelTsdfIntegrator::integrateVoxel(
538 | const Transformation& T_G_C, const Pointcloud& points_C,
539 | const Colors& colors, const Label& label, const bool enable_anti_grazing,
540 | const bool clearing_ray,
541 | const VoxelMapElement& global_voxel_idx_to_point_indices,
542 | const VoxelMap& voxel_map) {
543 | if (global_voxel_idx_to_point_indices.second.empty()) {
544 | return;
545 | }
546 |
547 | const Point& origin = T_G_C.getPosition();
548 | Color merged_color;
549 | Point merged_point_C = Point::Zero();
550 | FloatingPoint merged_weight = 0.0f;
551 | Label merged_label = label;
552 | LabelConfidence merged_label_confidence;
553 |
554 | for (const size_t pt_idx : global_voxel_idx_to_point_indices.second) {
555 | const Point& point_C = points_C[pt_idx];
556 | const Color& color = colors[pt_idx];
557 |
558 | const float point_weight = getVoxelWeight(point_C);
559 | merged_point_C = (merged_point_C * merged_weight + point_C * point_weight) /
560 | (merged_weight + point_weight);
561 | merged_color =
562 | Color::blendTwoColors(merged_color, merged_weight, color, point_weight);
563 | merged_weight += point_weight;
564 | // Assuming all the points of a segment pointcloud
565 | // are assigned the same label.
566 | if (label_tsdf_config_.enable_confidence_weight_dropoff) {
567 | const FloatingPoint ray_distance = point_C.norm();
568 | merged_label_confidence = computeConfidenceWeight(ray_distance);
569 | } else {
570 | merged_label_confidence = 1u;
571 | }
572 |
573 | // only take first point when clearing
574 | if (clearing_ray) {
575 | break;
576 | }
577 | }
578 |
579 | const Point merged_point_G = T_G_C * merged_point_C;
580 |
581 | RayCaster ray_caster(origin, merged_point_G, clearing_ray,
582 | config_.voxel_carving_enabled, config_.max_ray_length_m,
583 | voxel_size_inv_, config_.default_truncation_distance);
584 |
585 | GlobalIndex global_voxel_idx;
586 | while (ray_caster.nextRayIndex(&global_voxel_idx)) {
587 | if (enable_anti_grazing) {
588 | // Check if this one is already the block hash map for this
589 | // insertion. Skip this to avoid grazing.
590 | if ((clearing_ray ||
591 | global_voxel_idx != global_voxel_idx_to_point_indices.first) &&
592 | voxel_map.find(global_voxel_idx) != voxel_map.end()) {
593 | continue;
594 | }
595 | }
596 | BlockIndex block_idx;
597 |
598 | Block::Ptr tsdf_block = nullptr;
599 | TsdfVoxel* tsdf_voxel = allocateStorageAndGetVoxelPtr(
600 | global_voxel_idx, &tsdf_block, &block_idx);
601 |
602 | updateTsdfVoxel(origin, merged_point_G, global_voxel_idx, merged_color,
603 | merged_weight, tsdf_voxel);
604 |
605 | // TODO(margaritaG): parametrize.
606 | // If voxel carving is enabled, then only allocate the label voxels
607 | // within three times the truncation distance from the surface.
608 | if (!config_.voxel_carving_enabled ||
609 | std::abs(tsdf_voxel->distance) <
610 | 3 * config_.default_truncation_distance) {
611 | Block::Ptr label_block = nullptr;
612 | LabelVoxel* label_voxel = allocateStorageAndGetLabelVoxelPtr(
613 | global_voxel_idx, &label_block, &block_idx);
614 | updateLabelVoxel(merged_point_G, merged_label, label_voxel,
615 | merged_label_confidence);
616 | }
617 | }
618 | }
619 |
620 | void LabelTsdfIntegrator::integrateVoxels(
621 | const Transformation& T_G_C, const Pointcloud& points_C,
622 | const Colors& colors, const Label& label, const bool enable_anti_grazing,
623 | const bool clearing_ray, const VoxelMap& voxel_map,
624 | const VoxelMap& clear_map, const size_t thread_idx) {
625 | VoxelMap::const_iterator it;
626 | size_t map_size;
627 | if (clearing_ray) {
628 | it = clear_map.begin();
629 | map_size = clear_map.size();
630 | } else {
631 | it = voxel_map.begin();
632 | map_size = voxel_map.size();
633 | }
634 | for (size_t i = 0u; i < map_size; ++i) {
635 | if (((i + thread_idx + 1) % config_.integrator_threads) == 0u) {
636 | integrateVoxel(T_G_C, points_C, colors, label, enable_anti_grazing,
637 | clearing_ray, *it, voxel_map);
638 | }
639 | ++it;
640 | }
641 | }
642 |
643 | void LabelTsdfIntegrator::integrateRays(
644 | const Transformation& T_G_C, const Pointcloud& points_C,
645 | const Colors& colors, const Label& label, const bool enable_anti_grazing,
646 | const bool clearing_ray, const VoxelMap& voxel_map,
647 | const VoxelMap& clear_map) {
648 | const Point& origin = T_G_C.getPosition();
649 |
650 | // if only 1 thread just do function call, otherwise spawn threads
651 | if (config_.integrator_threads == 1u) {
652 | constexpr size_t thread_idx = 0u;
653 | integrateVoxels(T_G_C, points_C, colors, label, enable_anti_grazing,
654 | clearing_ray, voxel_map, clear_map, thread_idx);
655 | } else {
656 | std::list integration_threads;
657 | for (size_t i = 0u; i < config_.integrator_threads; ++i) {
658 | integration_threads.emplace_back(
659 | &LabelTsdfIntegrator::integrateVoxels, this, T_G_C,
660 | std::cref(points_C), std::cref(colors), label, enable_anti_grazing,
661 | clearing_ray, std::cref(voxel_map), std::cref(clear_map), i);
662 | }
663 |
664 | for (std::thread& thread : integration_threads) {
665 | thread.join();
666 | }
667 | }
668 |
669 | timing::Timer insertion_timer("inserting_missed_blocks");
670 | updateLayerWithStoredBlocks();
671 | updateLabelLayerWithStoredBlocks();
672 |
673 | insertion_timer.Stop();
674 | }
675 |
676 | FloatingPoint LabelTsdfIntegrator::computeConfidenceWeight(
677 | const FloatingPoint& distance) {
678 | const FloatingPoint mu = label_tsdf_config_.lognormal_weight_mean;
679 | const FloatingPoint sigma = label_tsdf_config_.lognormal_weight_sigma;
680 | FloatingPoint x =
681 | std::max(0.0f, distance - label_tsdf_config_.lognormal_weight_offset);
682 |
683 | if (x == 0.0f) {
684 | return 0.0f;
685 | }
686 | CHECK(sigma > 0.0f && std::isfinite(sigma))
687 | << "Scale parameter is " << sigma << ", but must be > 0 !";
688 | CHECK(std::isfinite(mu)) << "Location parameter is " << mu
689 | << ", but must be finite!";
690 | CHECK(x >= 0.0f && std::isfinite(x))
691 | << "Random variate is " << x << " but must be >= 0 !";
692 |
693 | FloatingPoint result = 0.0f;
694 | FloatingPoint exponent = std::log(x) - mu;
695 | exponent *= -exponent;
696 | exponent /= 2 * sigma * sigma;
697 |
698 | result = std::exp(exponent);
699 | result /= sigma * std::sqrt(2 * M_PI) * x;
700 |
701 | return result;
702 | }
703 |
704 | // Not thread safe.
705 | void LabelTsdfIntegrator::swapLabels(const Label& old_label,
706 | const Label& new_label) {
707 | BlockIndexList all_label_blocks;
708 | label_layer_->getAllAllocatedBlocks(&all_label_blocks);
709 |
710 | for (const BlockIndex& block_index : all_label_blocks) {
711 | Block::Ptr tsdf_block = layer_->getBlockPtrByIndex(block_index);
712 | Block::Ptr label_block =
713 | label_layer_->getBlockPtrByIndex(block_index);
714 | size_t vps = label_block->voxels_per_side();
715 | for (size_t i = 0u; i < vps * vps * vps; i++) {
716 | LabelVoxel& voxel = label_block->getVoxelByLinearIndex(i);
717 | Label previous_label = voxel.label;
718 |
719 | LabelConfidence old_label_confidence = 0u;
720 | for (LabelCount& label_count : voxel.label_count) {
721 | if (label_count.label == old_label) {
722 | // Store confidence for old_label and remove that entry.
723 | old_label_confidence = label_count.label_confidence;
724 | label_count.label = 0u;
725 | label_count.label_confidence = 0u;
726 | }
727 | }
728 | if (old_label_confidence > 0u) {
729 | // Add old_label confidence, if any, to new_label confidence.
730 | addVoxelLabelConfidence(new_label, old_label_confidence, &voxel);
731 | }
732 | // TODO(grinvalm) calling update with different preferred labels
733 | // can result in different assigned labels to the voxel, and
734 | // can trigger an update of a segment.
735 | updateVoxelLabelAndConfidence(&voxel, new_label);
736 | Label updated_label = voxel.label;
737 |
738 | if (updated_label != previous_label) {
739 | // The new updated_label gains a voxel.
740 | updated_labels_.insert(updated_label);
741 | changeLabelCount(updated_label, 1);
742 |
743 | changeLabelCount(previous_label, -1);
744 | if (!tsdf_block->updated()) {
745 | label_block->updated() = true;
746 | }
747 | }
748 | }
749 | }
750 | }
751 |
752 | void LabelTsdfIntegrator::resetCurrentFrameUpdatedLabelsAge() {
753 | for (const Label label : updated_labels_) {
754 | // Set timestamp or integer age of segment.
755 | // Here is the place to do it so it's the same timestamp
756 | // for all segments in a frame.
757 | labels_to_publish_[label] = 0;
758 | }
759 | updated_labels_.clear();
760 | }
761 |
762 | void LabelTsdfIntegrator::getLabelsToPublish(
763 | std::vector* segment_labels_to_publish) {
764 | CHECK_NOTNULL(segment_labels_to_publish);
765 | resetCurrentFrameUpdatedLabelsAge();
766 | clearCurrentFrameInstanceLabels();
767 |
768 | for (LMapIt label_age_pair_it = labels_to_publish_.begin();
769 | label_age_pair_it != labels_to_publish_.end();
770 | /* no increment */) {
771 | // Increase age of a label to publish;
772 | ++(label_age_pair_it)->second;
773 | if (label_age_pair_it->second > label_tsdf_config_.max_segment_age) {
774 | segment_labels_to_publish->push_back(label_age_pair_it->first);
775 | labels_to_publish_.erase(label_age_pair_it++);
776 | } else {
777 | ++label_age_pair_it;
778 | }
779 | }
780 | }
781 |
782 | void LabelTsdfIntegrator::addPairwiseConfidenceCount(
783 | const LLMapIt& label_map_it, const Label& label, const int count) {
784 | LMapIt label_count_it = label_map_it->second.find(label);
785 | if (label_count_it != label_map_it->second.end()) {
786 | // label_map already contains a pairwise confidence count
787 | // to label, increase existing value by count.
788 | label_map_it->second[label] = label_count_it->second + count;
789 | } else {
790 | label_map_it->second.emplace(label, count);
791 | }
792 | }
793 |
794 | void LabelTsdfIntegrator::adjustPairwiseConfidenceAfterMerging(
795 | const Label& new_label, const Label& old_label) {
796 | // Add all the pairwise confidence counts of the old_label to new_label.
797 | // First the counts (old_label -> some_label),
798 | // where old_label < some_label.
799 | LLMapIt old_label_pc_it = pairwise_confidence_.find(old_label);
800 | if (old_label_pc_it != pairwise_confidence_.end()) {
801 | LLMapIt new_label_pc_it = pairwise_confidence_.find(new_label);
802 | if (new_label_pc_it != pairwise_confidence_.end()) {
803 | for (LMapIt old_label_pc_count_it = old_label_pc_it->second.begin();
804 | old_label_pc_count_it != old_label_pc_it->second.end();
805 | ++old_label_pc_count_it) {
806 | addPairwiseConfidenceCount(new_label_pc_it,
807 | old_label_pc_count_it->first,
808 | old_label_pc_count_it->second);
809 | }
810 | } else {
811 | LMap old_label_map(old_label_pc_it->second.begin(),
812 | old_label_pc_it->second.end());
813 | pairwise_confidence_.emplace(new_label, old_label_map);
814 | }
815 | pairwise_confidence_.erase(old_label_pc_it);
816 | }
817 |
818 | // Next add the counts (some_label -> old_label),
819 | // where some_label < old_label.
820 | for (LLMapIt confidence_map_it = pairwise_confidence_.begin();
821 | confidence_map_it != pairwise_confidence_.end();
822 | /* no increment */) {
823 | for (LMapIt confidence_pair_it = confidence_map_it->second.begin();
824 | confidence_pair_it != confidence_map_it->second.end();
825 | /* no increment */) {
826 | if (confidence_pair_it->first == old_label) {
827 | if (confidence_map_it->first < new_label) {
828 | addPairwiseConfidenceCount(confidence_map_it, new_label,
829 | confidence_pair_it->second);
830 | } else {
831 | LLMapIt new_label_pc_it = pairwise_confidence_.find(new_label);
832 | if (new_label_pc_it != pairwise_confidence_.end()) {
833 | addPairwiseConfidenceCount(new_label_pc_it,
834 | confidence_map_it->first,
835 | confidence_pair_it->second);
836 | } else {
837 | LMap old_label_map;
838 | old_label_map.emplace(confidence_map_it->first,
839 | confidence_pair_it->second);
840 | pairwise_confidence_.emplace(new_label, old_label_map);
841 | }
842 | }
843 | confidence_pair_it =
844 | confidence_map_it->second.erase(confidence_pair_it);
845 | } else {
846 | ++confidence_pair_it;
847 | }
848 | }
849 | if (confidence_map_it->second.empty()) {
850 | confidence_map_it = pairwise_confidence_.erase(confidence_map_it);
851 | } else {
852 | ++confidence_map_it;
853 | }
854 | }
855 | }
856 |
857 | bool LabelTsdfIntegrator::getNextMerge(Label* new_label, Label* old_label) {
858 | CHECK_NOTNULL(new_label);
859 | CHECK_NOTNULL(old_label);
860 | for (LLMapIt confidence_map_it = pairwise_confidence_.begin();
861 | confidence_map_it != pairwise_confidence_.end(); ++confidence_map_it) {
862 | for (LMapIt confidence_pair_it = confidence_map_it->second.begin();
863 | confidence_pair_it != confidence_map_it->second.end();
864 | ++confidence_pair_it) {
865 | if (confidence_pair_it->second >
866 | label_tsdf_config_.merging_min_frame_count) {
867 | // If the pairwise confidence is above a threshold return
868 | // the two labels to merge and remove the pair
869 | // from the pairwise confidence counts.
870 | *new_label = confidence_map_it->first;
871 | *old_label = confidence_pair_it->first;
872 | confidence_pair_it =
873 | confidence_map_it->second.erase(confidence_pair_it);
874 | return true;
875 | }
876 | }
877 | }
878 | return false;
879 | }
880 |
881 | // Not thread safe.
882 | void LabelTsdfIntegrator::mergeLabels(LLSet* merges_to_publish) {
883 | CHECK_NOTNULL(merges_to_publish);
884 | if (label_tsdf_config_.enable_pairwise_confidence_merging) {
885 | Label new_label;
886 | Label old_label;
887 | while (getNextMerge(&new_label, &old_label)) {
888 | timing::Timer merge_timer("merge_segments");
889 | LOG(ERROR) << "Merging labels " << new_label << " and " << old_label;
890 | swapLabels(old_label, new_label);
891 |
892 | // Delete any staged segment publishing for overridden label.
893 | LMapIt label_age_pair_it = labels_to_publish_.find(old_label);
894 | if (label_age_pair_it != labels_to_publish_.end()) {
895 | labels_to_publish_.erase(old_label);
896 | }
897 | updated_labels_.erase(old_label);
898 |
899 | // Store the happened merge.
900 | LLSetIt label_it = merges_to_publish->find(new_label);
901 | if (label_it != merges_to_publish->end()) {
902 | // If the new_label already incorporated other labels
903 | // just add the just incorporated old_label to this list.
904 | label_it->second.emplace(old_label);
905 | } else {
906 | // If the new_label hasn't incorporated any other labels yet
907 | // create a new list and only add the just incorporated
908 | // old_label.
909 | std::set incorporated_labels;
910 | incorporated_labels.emplace(old_label);
911 | merges_to_publish->emplace(new_label, incorporated_labels);
912 | }
913 | adjustPairwiseConfidenceAfterMerging(new_label, old_label);
914 | merge_timer.Stop();
915 | }
916 | }
917 | }
918 |
919 | Transformation LabelTsdfIntegrator::getIcpRefined_T_G_C(
920 | const Transformation& T_G_C_init, const Pointcloud& point_cloud) {
921 | // TODO(ff): We should actually check here how many blocks are in the
922 | // camera frustum.
923 | if (layer_->getNumberOfAllocatedBlocks() <= 0u) {
924 | return T_G_C_init;
925 | }
926 | Transformation T_Gicp_C;
927 | timing::Timer icp_timer("icp");
928 | if (!label_tsdf_config_.keep_track_of_icp_correction) {
929 | T_Gicp_G_.setIdentity();
930 | }
931 |
932 | const size_t num_icp_updates =
933 | icp_->runICP(*layer_, point_cloud, T_Gicp_G_ * T_G_C_init, &T_Gicp_C);
934 | if (num_icp_updates == 0u ||
935 | num_icp_updates > label_tsdf_config_.max_num_icp_updates) {
936 | LOG(INFO) << "num_icp_updates is too high or 0: " << num_icp_updates
937 | << ", using T_G_C_init.";
938 | return T_G_C_init;
939 | }
940 | LOG(INFO) << "ICP refinement performed " << num_icp_updates
941 | << " successful update steps.";
942 | T_Gicp_G_ = T_Gicp_C * T_G_C_init.inverse();
943 |
944 | if (!label_tsdf_config_.keep_track_of_icp_correction) {
945 | LOG(INFO) << "Current ICP refinement offset: T_Gicp_G: " << T_Gicp_G_;
946 | } else {
947 | LOG(INFO) << "ICP refinement for this pointcloud: T_Gicp_G: " << T_Gicp_G_;
948 | }
949 |
950 | if (!icp_->refiningRollPitch()) {
951 | // its already removed internally but small floating point errors can
952 | // build up if accumulating transforms
953 | Transformation::Vector6 vec_T_Gicp_G = T_Gicp_G_.log();
954 | vec_T_Gicp_G[3] = 0.0;
955 | vec_T_Gicp_G[4] = 0.0;
956 | T_Gicp_G_ = Transformation::exp(vec_T_Gicp_G);
957 | }
958 |
959 | icp_timer.Stop();
960 | return T_Gicp_C;
961 | }
962 |
963 | } // namespace voxblox
964 |
--------------------------------------------------------------------------------
/global_segment_map/src/label_tsdf_map.cc:
--------------------------------------------------------------------------------
1 | #include "global_segment_map/label_tsdf_map.h"
2 |
3 | namespace voxblox {
4 |
5 | Labels LabelTsdfMap::getLabelList() {
6 | Labels labels;
7 | int count_unused_labels = 0;
8 | for (const std::pair& label_count_pair : label_count_map_) {
9 | if (label_count_pair.second > 0) {
10 | labels.push_back(label_count_pair.first);
11 | } else {
12 | count_unused_labels++;
13 | }
14 | }
15 | LOG(ERROR) << "Unused labels count: " << count_unused_labels;
16 | return labels;
17 | }
18 |
19 | InstanceLabels LabelTsdfMap::getInstanceList() {
20 | std::set instance_labels_set;
21 | Labels labels = getLabelList();
22 |
23 | float kFramesCountThresholdFactor = 0.1f;
24 |
25 | for (const Label label : labels) {
26 | InstanceLabel instance_label =
27 | semantic_instance_label_fusion_.getInstanceLabel(
28 | label, kFramesCountThresholdFactor);
29 | if (instance_label != 0u) {
30 | instance_labels_set.emplace(instance_label);
31 | }
32 | }
33 | InstanceLabels instance_labels(instance_labels_set.begin(),
34 | instance_labels_set.end());
35 |
36 | return instance_labels;
37 | }
38 |
39 | void LabelTsdfMap::getSemanticInstanceList(InstanceLabels* instance_labels,
40 | SemanticLabels* semantic_labels) {
41 | std::set instance_labels_set;
42 | Labels labels = getLabelList();
43 |
44 | float kFramesCountThresholdFactor = 0.1f;
45 |
46 | for (const Label label : labels) {
47 | InstanceLabel instance_label =
48 | semantic_instance_label_fusion_.getInstanceLabel(
49 | label, kFramesCountThresholdFactor);
50 |
51 | // If the label maps to an instance,
52 | // fetch the corresponding semantic class.
53 | if (instance_label != 0u) {
54 | // As multiple labels can match to a same instance_label,
55 | // make sure to only add once each instance_label.
56 | auto ret = instance_labels_set.emplace(instance_label);
57 | if (ret.second) {
58 | SemanticLabel semantic_label =
59 | semantic_instance_label_fusion_.getSemanticLabel(label);
60 | CHECK_NE(semantic_label, 0u)
61 | << "Instance assigned to semantic category BACKGROUND.";
62 | instance_labels->push_back(instance_label);
63 | semantic_labels->push_back(semantic_label);
64 | }
65 | }
66 | }
67 | }
68 |
69 | void LabelTsdfMap::extractSegmentLayers(
70 | const std::vector& labels,
71 | std::unordered_map* label_layers_map,
72 | const bool labels_list_is_complete) {
73 | CHECK_NOTNULL(label_layers_map);
74 |
75 | // Map a label to its corresponding TSDF and label layers.
76 | Layer tsdf_layer_empty(config_.voxel_size,
77 | config_.voxels_per_side);
78 | Layer label_layer_empty(config_.voxel_size,
79 | config_.voxels_per_side);
80 | for (const Label& label : labels) {
81 | label_layers_map->emplace(
82 | label, std::make_pair(tsdf_layer_empty, label_layer_empty));
83 | }
84 |
85 | BlockIndexList all_label_blocks;
86 | tsdf_layer_->getAllAllocatedBlocks(&all_label_blocks);
87 |
88 | for (const BlockIndex& block_index : all_label_blocks) {
89 | Block::Ptr global_tsdf_block =
90 | tsdf_layer_->getBlockPtrByIndex(block_index);
91 | Block::Ptr global_label_block =
92 | label_layer_->getBlockPtrByIndex(block_index);
93 |
94 | const size_t vps = global_label_block->voxels_per_side();
95 | for (size_t i = 0u; i < vps * vps * vps; ++i) {
96 | const LabelVoxel& global_label_voxel =
97 | global_label_block->getVoxelByLinearIndex(i);
98 |
99 | if (global_label_voxel.label == 0u) {
100 | continue;
101 | }
102 |
103 | auto it = label_layers_map->find(global_label_voxel.label);
104 | if (it == label_layers_map->end()) {
105 | if (labels_list_is_complete) {
106 | // TODO(margaritaG): this seemed to fail once, find out why there are
107 | // labels in the map which are not in the label list.
108 | LOG(FATAL) << "At least one voxel in the GSM is assigned to label "
109 | << global_label_voxel.label
110 | << " which is not in the given "
111 | "list of labels to retrieve.";
112 | }
113 | continue;
114 | }
115 |
116 | Layer& tsdf_layer = it->second.first;
117 | Layer& label_layer = it->second.second;
118 |
119 | Block::Ptr tsdf_block =
120 | tsdf_layer.allocateBlockPtrByIndex(block_index);
121 | Block::Ptr label_block =
122 | label_layer.allocateBlockPtrByIndex(block_index);
123 | CHECK(tsdf_block);
124 | CHECK(label_block);
125 |
126 | TsdfVoxel& tsdf_voxel = tsdf_block->getVoxelByLinearIndex(i);
127 | LabelVoxel& label_voxel = label_block->getVoxelByLinearIndex(i);
128 |
129 | const TsdfVoxel& global_tsdf_voxel =
130 | global_tsdf_block->getVoxelByLinearIndex(i);
131 |
132 | tsdf_voxel = global_tsdf_voxel;
133 | label_voxel = global_label_voxel;
134 | }
135 | }
136 | }
137 |
138 | void LabelTsdfMap::extractInstanceLayers(
139 | const InstanceLabels& instance_labels,
140 | std::unordered_map* instance_layers_map) {
141 | CHECK_NOTNULL(instance_layers_map);
142 | // Map an instance label to its corresponding TSDF and label layers.
143 | Layer tsdf_layer_empty(config_.voxel_size,
144 | config_.voxels_per_side);
145 | Layer label_layer_empty(config_.voxel_size,
146 | config_.voxels_per_side);
147 | for (const InstanceLabel& instance_label : instance_labels) {
148 | instance_layers_map->emplace(
149 | instance_label, std::make_pair(tsdf_layer_empty, label_layer_empty));
150 | }
151 |
152 | BlockIndexList all_label_blocks;
153 | tsdf_layer_->getAllAllocatedBlocks(&all_label_blocks);
154 |
155 | for (const BlockIndex& block_index : all_label_blocks) {
156 | Block::Ptr global_tsdf_block =
157 | tsdf_layer_->getBlockPtrByIndex(block_index);
158 | Block::Ptr global_label_block =
159 | label_layer_->getBlockPtrByIndex(block_index);
160 |
161 | const size_t vps = global_label_block->voxels_per_side();
162 | for (size_t i = 0u; i < vps * vps * vps; ++i) {
163 | const LabelVoxel& global_label_voxel =
164 | global_label_block->getVoxelByLinearIndex(i);
165 |
166 | if (global_label_voxel.label == 0u) {
167 | continue;
168 | }
169 | float kFramesCountThresholdFactor = 0.1f;
170 | InstanceLabel instance_label =
171 | semantic_instance_label_fusion_.getInstanceLabel(
172 | global_label_voxel.label, kFramesCountThresholdFactor);
173 |
174 | if (instance_label == 0u) {
175 | continue;
176 | }
177 |
178 | auto it = instance_layers_map->find(instance_label);
179 | if (it == instance_layers_map->end()) {
180 | continue;
181 | }
182 |
183 | Layer& tsdf_layer = it->second.first;
184 | Layer& label_layer = it->second.second;
185 |
186 | Block::Ptr tsdf_block =
187 | tsdf_layer.allocateBlockPtrByIndex(block_index);
188 | Block::Ptr label_block =
189 | label_layer.allocateBlockPtrByIndex(block_index);
190 | CHECK(tsdf_block);
191 | CHECK(label_block);
192 |
193 | TsdfVoxel& tsdf_voxel = tsdf_block->getVoxelByLinearIndex(i);
194 | LabelVoxel& label_voxel = label_block->getVoxelByLinearIndex(i);
195 |
196 | const TsdfVoxel& global_tsdf_voxel =
197 | global_tsdf_block->getVoxelByLinearIndex(i);
198 |
199 | tsdf_voxel = global_tsdf_voxel;
200 | label_voxel = global_label_voxel;
201 | }
202 | }
203 | }
204 |
205 | } // namespace voxblox
206 |
--------------------------------------------------------------------------------
/global_segment_map/src/meshing/instance_color_map.cc:
--------------------------------------------------------------------------------
1 | #include "global_segment_map/meshing/instance_color_map.h"
2 |
3 | #include "voxblox/core/color.h"
4 |
5 | namespace voxblox {
6 |
7 | void InstanceColorMap::getColor(const InstanceLabel& instance_label,
8 | Color* color) {
9 | CHECK_NOTNULL(color);
10 | std::map::iterator instance_color_map_it;
11 | {
12 | std::shared_lock readerLock(color_map_mutex_);
13 | instance_color_map_it = color_map_.find(instance_label);
14 | }
15 |
16 | if (instance_color_map_it != color_map_.end()) {
17 | *color = instance_color_map_it->second;
18 | } else {
19 | if (instance_label == 0u) {
20 | // TODO(margaritaG): parametrize the grey color.
21 | color->r = 200u;
22 | color->g = 200u;
23 | color->b = 200u;
24 | } else {
25 | *color = randomColor();
26 | }
27 | std::lock_guard writerLock(color_map_mutex_);
28 | color_map_.insert(std::pair(instance_label, *color));
29 | }
30 | }
31 | } // namespace voxblox
32 |
--------------------------------------------------------------------------------
/global_segment_map/src/meshing/label_color_map.cc:
--------------------------------------------------------------------------------
1 | #include "global_segment_map/meshing/label_color_map.h"
2 |
3 | #include "voxblox/core/color.h"
4 |
5 | namespace voxblox {
6 |
7 | void LabelColorMap::getColor(const Label& label, Color* color) {
8 | CHECK_NOTNULL(color);
9 | CHECK_NE(label, 0u);
10 |
11 | std::map::iterator label_color_map_it;
12 | {
13 | std::shared_lock readerLock(color_map_mutex_);
14 | label_color_map_it = color_map_.find(label);
15 | }
16 |
17 | if (label_color_map_it != color_map_.end()) {
18 | *color = label_color_map_it->second;
19 | } else {
20 | *color = randomColor();
21 |
22 | std::lock_guard writerLock(color_map_mutex_);
23 | color_map_.insert(std::pair(label, *color));
24 | }
25 | }
26 | } // namespace voxblox
27 |
--------------------------------------------------------------------------------
/global_segment_map/src/meshing/label_tsdf_mesh_integrator.cc:
--------------------------------------------------------------------------------
1 | // NOTE: Code adapted from open_chisel:
2 | // github.com/personalrobotics/OpenChisel/
3 |
4 | // The MIT License (MIT) Copyright (c)
5 | // 2014 Matthew Klingensmith and Ivan Dryanovski
6 | //
7 | // Permission is hereby granted, free of charge, to any person obtaining a copy
8 | // of this software and associated documentation files (the "Software"), to deal
9 | // in the Software without restriction, including without limitation the rights
10 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11 | // copies of the Software, and to permit persons to whom the Software is
12 | // furnished to do so, subject to the following conditions:
13 | //
14 | // The above copyright notice and this permission notice shall be included in
15 | // all
16 | // copies or substantial portions of the Software.
17 | //
18 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24 | // SOFTWARE.
25 |
26 | #include "global_segment_map/meshing/label_tsdf_mesh_integrator.h"
27 |
28 | #include "voxblox/core/voxel.h"
29 |
30 | namespace voxblox {
31 | MeshLabelIntegrator::MeshLabelIntegrator(
32 | const MeshIntegratorConfig& config,
33 | const MeshLabelIntegrator::LabelTsdfConfig& label_tsdf_config,
34 | LabelTsdfMap* map, MeshLayer* mesh_layer, bool* remesh)
35 | : MeshIntegrator(config, CHECK_NOTNULL(map)->getTsdfLayerPtr(), mesh_layer),
36 | label_tsdf_config_(label_tsdf_config),
37 | label_layer_mutable_ptr_(CHECK_NOTNULL(map->getLabelLayerPtr())),
38 | label_layer_const_ptr_(CHECK_NOTNULL(map->getLabelLayerPtr())),
39 | semantic_instance_label_fusion_ptr_(
40 | map->getSemanticInstanceLabelFusionPtr()),
41 | label_color_map_(),
42 | instance_color_map_(),
43 | semantic_color_map_(
44 | SemanticColorMap::create(label_tsdf_config.class_task)),
45 | remesh_ptr_(remesh) {
46 | if (remesh_ptr_ == nullptr) {
47 | remesh_ptr_ = &remesh_;
48 | }
49 | }
50 |
51 | MeshLabelIntegrator::MeshLabelIntegrator(
52 | const MeshIntegratorConfig& config,
53 | const MeshLabelIntegrator::LabelTsdfConfig& label_tsdf_config,
54 | const LabelTsdfMap& map, MeshLayer* mesh_layer, bool* remesh)
55 | : MeshIntegrator(config, map.getTsdfLayer(), mesh_layer),
56 | label_tsdf_config_(label_tsdf_config),
57 | label_layer_mutable_ptr_(nullptr),
58 | label_layer_const_ptr_(CHECK_NOTNULL(&map.getLabelLayer())),
59 | semantic_instance_label_fusion_ptr_(
60 | &map.getSemanticInstanceLabelFusion()),
61 | label_color_map_(),
62 | instance_color_map_(),
63 | semantic_color_map_(
64 | SemanticColorMap::create(label_tsdf_config.class_task)),
65 | remesh_ptr_(remesh) {
66 | if (remesh_ptr_ == nullptr) {
67 | remesh_ptr_ = &remesh_;
68 | }
69 | }
70 |
71 | MeshLabelIntegrator::MeshLabelIntegrator(
72 | const MeshIntegratorConfig& config,
73 | const MeshLabelIntegrator::LabelTsdfConfig& label_tsdf_config,
74 | const Layer& tsdf_layer, const Layer& label_layer,
75 | MeshLayer* mesh_layer)
76 | : MeshIntegrator(config, tsdf_layer, mesh_layer),
77 | label_tsdf_config_(label_tsdf_config),
78 | label_layer_mutable_ptr_(nullptr),
79 | label_layer_const_ptr_(&label_layer),
80 | semantic_instance_label_fusion_ptr_(nullptr),
81 | label_color_map_(),
82 | instance_color_map_(),
83 | semantic_color_map_(
84 | SemanticColorMap::create(label_tsdf_config.class_task)) {}
85 |
86 | bool MeshLabelIntegrator::generateMesh(bool only_mesh_updated_blocks,
87 | bool clear_updated_flag) {
88 | CHECK(!clear_updated_flag || ((sdf_layer_mutable_ != nullptr) &&
89 | (label_layer_mutable_ptr_ != nullptr)))
90 | << "If you would like to modify the updated flag in the blocks, please "
91 | << "use the constructor that provides a non-const link to the sdf and "
92 | "label layers!";
93 |
94 | BlockIndexList all_tsdf_blocks;
95 |
96 | if (only_mesh_updated_blocks) {
97 | BlockIndexList all_label_blocks;
98 | sdf_layer_const_->getAllUpdatedBlocks(&all_tsdf_blocks);
99 | label_layer_mutable_ptr_->getAllUpdatedBlocks(&all_label_blocks);
100 | if (all_tsdf_blocks.size() == 0u && all_label_blocks.size() == 0u) {
101 | return false;
102 | }
103 |
104 | all_tsdf_blocks.insert(all_tsdf_blocks.end(), all_label_blocks.begin(),
105 | all_label_blocks.end());
106 | } else {
107 | sdf_layer_const_->getAllAllocatedBlocks(&all_tsdf_blocks);
108 | }
109 |
110 | // Allocate all the mesh memory
111 | for (const BlockIndex& block_index : all_tsdf_blocks) {
112 | mesh_layer_->allocateMeshPtrByIndex(block_index);
113 | }
114 |
115 | std::unique_ptr index_getter(
116 | new MixedThreadSafeIndex(all_tsdf_blocks.size()));
117 |
118 | std::list integration_threads;
119 | for (size_t i = 0u; i < config_.integrator_threads; ++i) {
120 | integration_threads.emplace_back(
121 | &MeshLabelIntegrator::generateMeshBlocksFunction, this,
122 | std::cref(all_tsdf_blocks), clear_updated_flag, index_getter.get());
123 | }
124 |
125 | for (std::thread& thread : integration_threads) {
126 | thread.join();
127 | }
128 |
129 | return true;
130 | }
131 |
132 | void MeshLabelIntegrator::generateMeshBlocksFunction(
133 | const BlockIndexList& all_tsdf_blocks, bool clear_updated_flag,
134 | ThreadSafeIndex* index_getter) {
135 | CHECK(!clear_updated_flag || (sdf_layer_mutable_ != nullptr) ||
136 | (label_layer_mutable_ptr_ != nullptr))
137 | << "If you would like to modify the updated flag in the blocks, please "
138 | << "use the constructor that provides a non-const link to the sdf and "
139 | "label layers!";
140 | CHECK_NOTNULL(index_getter);
141 |
142 | size_t list_idx;
143 | while (index_getter->getNextIndex(&list_idx)) {
144 | const BlockIndex& block_idx = all_tsdf_blocks[list_idx];
145 | updateMeshForBlock(block_idx);
146 | if (clear_updated_flag) {
147 | typename Block::Ptr tsdf_block =
148 | sdf_layer_mutable_->getBlockPtrByIndex(block_idx);
149 | typename Block::Ptr label_block =
150 | label_layer_mutable_ptr_->getBlockPtrByIndex(block_idx);
151 |
152 | tsdf_block->updated() = false;
153 | label_block->updated() = false;
154 | }
155 | }
156 | }
157 |
158 | // TODO(margaritaG): handle this remeshing!!
159 | InstanceLabel MeshLabelIntegrator::getInstanceLabel(const Label& label) {
160 | float kFramesCountThresholdFactor = 0.1f;
161 |
162 | InstanceLabel instance_label =
163 | semantic_instance_label_fusion_ptr_->getInstanceLabel(
164 | label, kFramesCountThresholdFactor);
165 | std::map::iterator prev_instance_it;
166 | {
167 | std::shared_lock readerLock(
168 | label_instance_map_mutex_);
169 | prev_instance_it = label_instance_map_.find(label);
170 | }
171 |
172 | if (prev_instance_it != label_instance_map_.end()) {
173 | if (prev_instance_it->second != instance_label) {
174 | *remesh_ptr_ = true;
175 | }
176 | }
177 | std::lock_guard writerLock(
178 | label_instance_map_mutex_);
179 | label_instance_map_[label] = instance_label;
180 | return instance_label;
181 | }
182 |
183 | void MeshLabelIntegrator::updateMeshForBlock(const BlockIndex& block_index) {
184 | Mesh::Ptr mesh_block = mesh_layer_->getMeshPtrByIndex(block_index);
185 | mesh_block->clear();
186 | // This block should already exist, otherwise it makes no sense to update
187 | // the mesh for it. ;)
188 | Block
10 |