24 #include "glog/logging.h" 27 namespace mapping_3d {
31 constexpr
float kSliceHalfHeight = 0.1f;
49 void GenerateSegmentForSlice(
const sensor::RangeData& range_data,
52 std::vector<RaySegment>* segments) {
53 const sensor::RangeData transformed_range_data =
55 segments->reserve(transformed_range_data.returns.size());
56 for (
const Eigen::Vector3f&
hit : transformed_range_data.returns) {
57 const Eigen::Vector2f origin_xy = transformed_range_data.origin.head<2>();
58 const float origin_z = transformed_range_data.origin.z();
59 const float delta_z = hit.z() - origin_z;
60 const Eigen::Vector2f delta_xy = hit.head<2>() - origin_xy;
61 if (origin_z < -kSliceHalfHeight) {
63 if (hit.z() > kSliceHalfHeight) {
65 segments->push_back(RaySegment{
66 origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
67 origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
69 }
else if (hit.z() > -kSliceHalfHeight) {
71 segments->push_back(RaySegment{
72 origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
73 hit.head<2>(),
true});
75 }
else if (origin_z < kSliceHalfHeight) {
77 if (hit.z() < -kSliceHalfHeight) {
79 segments->push_back(RaySegment{
81 origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
83 }
else if (hit.z() < kSliceHalfHeight) {
85 segments->push_back(RaySegment{origin_xy, hit.head<2>(),
true});
88 segments->push_back(RaySegment{
90 origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
95 if (hit.z() < -kSliceHalfHeight) {
97 segments->push_back(RaySegment{
98 origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
99 origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
101 }
else if (hit.z() < kSliceHalfHeight) {
103 segments->push_back(RaySegment{
104 origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
105 hit.head<2>(),
true});
111 void UpdateFreeSpaceFromSegment(
const RaySegment& segment,
112 const std::vector<uint16>& miss_table,
113 mapping_2d::ProbabilityGrid* result) {
114 Eigen::Array2i
from = result->limits().GetXYIndexOfCellContainingPoint(
115 segment.from.x(), segment.from.y());
116 Eigen::Array2i
to = result->limits().GetXYIndexOfCellContainingPoint(
117 segment.to.x(), segment.to.y());
119 std::abs(to.y() - from.y()) > std::abs(to.x() - from.x());
121 std::swap(from.x(), from.y());
122 std::swap(to.x(), to.y());
124 if (from.x() > to.x()) {
127 const int dx = to.x() - from.x();
128 const int dy = std::abs(to.y() - from.y());
130 const int direction = (from.y() < to.y()) ? 1 : -1;
132 for (; from.x() < to.x(); ++from.x()) {
134 result->ApplyLookupTable(Eigen::Array2i(from.y(), from.x()), miss_table);
136 result->ApplyLookupTable(from, miss_table);
140 from.y() += direction;
146 void InsertSegmentsIntoProbabilityGrid(
const std::vector<RaySegment>& segments,
147 const std::vector<uint16>& hit_table,
148 const std::vector<uint16>& miss_table,
149 mapping_2d::ProbabilityGrid* result) {
150 result->StartUpdate();
151 if (segments.empty()) {
154 Eigen::Vector2f min = segments.front().from;
155 Eigen::Vector2f max = min;
156 for (
const RaySegment& segment : segments) {
157 min = min.cwiseMin(segment.from);
158 min = min.cwiseMin(segment.to);
159 max = max.cwiseMax(segment.from);
160 max = max.cwiseMax(segment.to);
162 const float padding = 10. * result->limits().resolution();
163 max += Eigen::Vector2f(padding, padding);
164 min -= Eigen::Vector2f(padding, padding);
165 result->GrowLimits(min.x(), min.y());
166 result->GrowLimits(max.x(), max.y());
168 for (
const RaySegment& segment : segments) {
170 result->ApplyLookupTable(result->limits().GetXYIndexOfCellContainingPoint(
171 segment.to.x(), segment.to.y()),
175 for (
const RaySegment& segment : segments) {
176 UpdateFreeSpaceFromSegment(segment, miss_table, result);
183 sensor::RangeData FilterRangeDataByMaxRange(
const sensor::RangeData& range_data,
184 const float max_range) {
185 sensor::RangeData result{range_data.origin, {}, {}};
186 for (
const Eigen::Vector3f&
hit : range_data.returns) {
187 if ((hit - range_data.origin).norm() <= max_range) {
188 result.returns.push_back(hit);
194 std::vector<PixelData> AccumulatePixelData(
195 const int width,
const int height,
const Eigen::Array2i& min_index,
196 const Eigen::Array2i& max_index,
197 const std::vector<Eigen::Array4i>& voxel_indices_and_probabilities) {
198 std::vector<PixelData> accumulated_pixel_data(width * height);
199 for (
const Eigen::Array4i& voxel_index_and_probability :
200 voxel_indices_and_probabilities) {
201 const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
202 if ((pixel_index < min_index).any() || (pixel_index > max_index).any()) {
206 const int x = max_index.x() - pixel_index[0];
207 const int y = max_index.y() - pixel_index[1];
208 PixelData& pixel = accumulated_pixel_data[x * width + y];
210 pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]);
211 pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]);
212 const float probability =
214 pixel.probability_sum += probability;
215 pixel.max_probability = std::max(pixel.max_probability, probability);
217 return accumulated_pixel_data;
223 std::vector<Eigen::Array4i> ExtractVoxelData(
225 Eigen::Array2i* min_index, Eigen::Array2i* max_index) {
226 std::vector<Eigen::Array4i> voxel_indices_and_probabilities;
227 const float resolution_inverse = 1.f / hybrid_grid.resolution();
229 constexpr
float kXrayObstructedCellProbabilityLimit = 0.501f;
231 const uint16 probability_value = it.GetValue();
233 if (probability < kXrayObstructedCellProbabilityLimit) {
238 const Eigen::Vector3f cell_center_submap =
239 hybrid_grid.GetCenterOfCell(it.GetCellIndex());
240 const Eigen::Vector3f cell_center_global = transform * cell_center_submap;
241 const Eigen::Array4i voxel_index_and_probability(
247 voxel_indices_and_probabilities.push_back(voxel_index_and_probability);
248 const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
249 *min_index = min_index->cwiseMin(pixel_index);
250 *max_index = max_index->cwiseMax(pixel_index);
252 return voxel_indices_and_probabilities;
257 string ComputePixelValues(
258 const std::vector<PixelData>& accumulated_pixel_data) {
260 cell_data.reserve(2 * accumulated_pixel_data.size());
261 constexpr
float kMinZDifference = 3.f;
262 constexpr
float kFreeSpaceWeight = 0.15f;
263 for (
const PixelData& pixel : accumulated_pixel_data) {
267 const float z_difference = pixel.count > 0 ? pixel.max_z - pixel.min_z : 0;
268 if (z_difference < kMinZDifference) {
269 cell_data.push_back(0);
270 cell_data.push_back(0);
273 const float free_space = std::max(z_difference - pixel.count, 0.f);
274 const float free_space_weight = kFreeSpaceWeight * free_space;
275 const float total_weight = pixel.count + free_space_weight;
276 const float free_space_probability = 1.f - pixel.max_probability;
278 (pixel.probability_sum + free_space_probability * free_space_weight) /
282 const uint8 alpha = delta > 0 ? 0 : -delta;
284 cell_data.push_back(value);
285 cell_data.push_back((value || alpha) ? alpha : 1);
297 std::vector<RaySegment> segments;
298 GenerateSegmentForSlice(
302 InsertSegmentsIntoProbabilityGrid(segments, range_data_inserter.
hit_table(),
308 proto::SubmapsOptions options;
309 options.set_high_resolution(
310 parameter_dictionary->
GetDouble(
"high_resolution"));
311 options.set_high_resolution_max_range(
312 parameter_dictionary->
GetDouble(
"high_resolution_max_range"));
313 options.set_low_resolution(parameter_dictionary->
GetDouble(
"low_resolution"));
314 options.set_num_range_data(
316 *options.mutable_range_data_inserter_options() =
318 parameter_dictionary->
GetDictionary(
"range_data_inserter").get());
319 CHECK_GT(options.num_range_data(), 0);
325 : mapping::
Submap(local_pose),
326 high_resolution_hybrid_grid_(high_resolution),
327 low_resolution_hybrid_grid_(low_resolution) {}
331 mapping::proto::SubmapQuery::Response*
const response)
const {
336 response->set_resolution(resolution);
339 Eigen::Array2i min_index(INT_MAX, INT_MAX);
340 Eigen::Array2i max_index(INT_MIN, INT_MIN);
341 const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
343 global_submap_pose.
cast<
float>(), &min_index,
346 const int width = max_index.y() - min_index.y() + 1;
347 const int height = max_index.x() - min_index.x() + 1;
348 response->set_width(width);
349 response->set_height(height);
351 const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
352 width, height, min_index, max_index, voxel_indices_and_probabilities);
353 const string cell_data = ComputePixelValues(accumulated_pixel_data);
359 max_index.x() * resolution, max_index.y() * resolution,
376 CHECK_LT(index,
size());
383 const Eigen::Quaterniond& gravity_alignment) {
389 FilterRangeDataByMaxRange(transformed_range_data,
390 options_.high_resolution_max_range()),
410 options_.low_resolution(), local_pose));
411 LOG(INFO) <<
"Added submap " <<
size();
size_t num_range_data() const
proto::RangeDataInserterOptions options_
void FastGzipString(const string &uncompressed, string *compressed)
float ClampProbability(const float probability)
int GetNonNegativeInt(const string &key)
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(common::LuaParameterDictionary *parameter_dictionary)
void ToResponseProto(const transform::Rigid3d &global_submap_pose, mapping::proto::SubmapQuery::Response *response) const override
int RoundToInt(const float x)
HybridGrid low_resolution_hybrid_grid_
RangeDataInserter range_data_inserter_
transform::Rigid3d local_pose() const
void AddSubmap(const transform::Rigid3d &local_pose)
Submap(float high_resolution, float low_resolution, const transform::Rigid3d &local_pose)
std::vector< int > insertion_indices() const
double GetDouble(const string &key)
float ValueToProbability(const uint16 value)
RangeData TransformRangeData(const RangeData &range_data, const transform::Rigid3f &transform)
std::vector< std::unique_ptr< Submap > > submaps_
proto::SubmapsOptions CreateSubmapsOptions(common::LuaParameterDictionary *parameter_dictionary)
uint8 ProbabilityToLogOddsInteger(const float probability)
typename Grid< uint16 >::Iterator Iterator
void InsertRangeData(const sensor::RangeData &range_data, const Eigen::Quaterniond &gravity_alignment)
HybridGrid high_resolution_hybrid_grid_
const std::vector< uint16 > & hit_table() const
const Submap * Get(int index) const override
const proto::SubmapsOptions options_
const std::vector< uint16 > & miss_table() const
void InsertIntoProbabilityGrid(const sensor::RangeData &range_data, const transform::Rigid3f &pose, const float slice_z, const mapping_2d::RangeDataInserter &range_data_inserter, mapping_2d::ProbabilityGrid *result)
std::unique_ptr< RangeDataInserter > range_data_inserter_
std::unique_ptr< LuaParameterDictionary > GetDictionary(const string &key)
void Insert(const sensor::RangeData &range_data, HybridGrid *hybrid_grid) const
int size() const override