Remove HybridGrid slicing into a ProbabilityGrid. (#385)
This code is currently unused. The feature of slicing a HybridGrid into a Probability grid could be useful to for example collect a map in 3D, then slice the 3D data to create a 2D map. There are simpler ways to implement this, especially in the context of the assets_writer_main. PAIR=wohemaster
Holger Rapp
Wolfgang Hess
parent
1e6723e214
commit
611f244d7a
|
|
@ -28,14 +28,6 @@ namespace mapping_3d {
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
constexpr float kSliceHalfHeight = 0.1f;
|
|
||||||
|
|
||||||
struct RaySegment {
|
|
||||||
Eigen::Vector2f from;
|
|
||||||
Eigen::Vector2f to;
|
|
||||||
bool hit; // Whether there is a hit at 'to'.
|
|
||||||
};
|
|
||||||
|
|
||||||
struct PixelData {
|
struct PixelData {
|
||||||
int min_z = INT_MAX;
|
int min_z = INT_MAX;
|
||||||
int max_z = INT_MIN;
|
int max_z = INT_MIN;
|
||||||
|
|
@ -44,139 +36,6 @@ struct PixelData {
|
||||||
float max_probability = 0.5f;
|
float max_probability = 0.5f;
|
||||||
};
|
};
|
||||||
|
|
||||||
// We compute a slice around the xy-plane. 'transform' is applied to the rays in
|
|
||||||
// global map frame to allow choosing an arbitrary slice.
|
|
||||||
void GenerateSegmentForSlice(const sensor::RangeData& range_data,
|
|
||||||
const transform::Rigid3f& pose,
|
|
||||||
const transform::Rigid3f& transform,
|
|
||||||
std::vector<RaySegment>* segments) {
|
|
||||||
const sensor::RangeData transformed_range_data =
|
|
||||||
sensor::TransformRangeData(range_data, transform * pose);
|
|
||||||
segments->reserve(transformed_range_data.returns.size());
|
|
||||||
for (const Eigen::Vector3f& hit : transformed_range_data.returns) {
|
|
||||||
const Eigen::Vector2f origin_xy = transformed_range_data.origin.head<2>();
|
|
||||||
const float origin_z = transformed_range_data.origin.z();
|
|
||||||
const float delta_z = hit.z() - origin_z;
|
|
||||||
const Eigen::Vector2f delta_xy = hit.head<2>() - origin_xy;
|
|
||||||
if (origin_z < -kSliceHalfHeight) {
|
|
||||||
// Ray originates below the slice.
|
|
||||||
if (hit.z() > kSliceHalfHeight) {
|
|
||||||
// Ray is cutting through the slice.
|
|
||||||
segments->push_back(RaySegment{
|
|
||||||
origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
|
|
||||||
origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
|
|
||||||
false});
|
|
||||||
} else if (hit.z() > -kSliceHalfHeight) {
|
|
||||||
// Hit is inside the slice.
|
|
||||||
segments->push_back(RaySegment{
|
|
||||||
origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
|
|
||||||
hit.head<2>(), true});
|
|
||||||
}
|
|
||||||
} else if (origin_z < kSliceHalfHeight) {
|
|
||||||
// Ray originates inside the slice.
|
|
||||||
if (hit.z() < -kSliceHalfHeight) {
|
|
||||||
// Hit is below.
|
|
||||||
segments->push_back(RaySegment{
|
|
||||||
origin_xy,
|
|
||||||
origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
|
|
||||||
false});
|
|
||||||
} else if (hit.z() < kSliceHalfHeight) {
|
|
||||||
// Full ray is inside the slice.
|
|
||||||
segments->push_back(RaySegment{origin_xy, hit.head<2>(), true});
|
|
||||||
} else {
|
|
||||||
// Hit is above.
|
|
||||||
segments->push_back(RaySegment{
|
|
||||||
origin_xy,
|
|
||||||
origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
|
|
||||||
false});
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// Ray originates above the slice.
|
|
||||||
if (hit.z() < -kSliceHalfHeight) {
|
|
||||||
// Ray is cutting through the slice.
|
|
||||||
segments->push_back(RaySegment{
|
|
||||||
origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
|
|
||||||
origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy,
|
|
||||||
false});
|
|
||||||
} else if (hit.z() < kSliceHalfHeight) {
|
|
||||||
// Hit is inside the slice.
|
|
||||||
segments->push_back(RaySegment{
|
|
||||||
origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy,
|
|
||||||
hit.head<2>(), true});
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void UpdateFreeSpaceFromSegment(const RaySegment& segment,
|
|
||||||
const std::vector<uint16>& miss_table,
|
|
||||||
mapping_2d::ProbabilityGrid* result) {
|
|
||||||
Eigen::Array2i from = result->limits().GetXYIndexOfCellContainingPoint(
|
|
||||||
segment.from.x(), segment.from.y());
|
|
||||||
Eigen::Array2i to = result->limits().GetXYIndexOfCellContainingPoint(
|
|
||||||
segment.to.x(), segment.to.y());
|
|
||||||
bool large_delta_y =
|
|
||||||
std::abs(to.y() - from.y()) > std::abs(to.x() - from.x());
|
|
||||||
if (large_delta_y) {
|
|
||||||
std::swap(from.x(), from.y());
|
|
||||||
std::swap(to.x(), to.y());
|
|
||||||
}
|
|
||||||
if (from.x() > to.x()) {
|
|
||||||
std::swap(from, to);
|
|
||||||
}
|
|
||||||
const int dx = to.x() - from.x();
|
|
||||||
const int dy = std::abs(to.y() - from.y());
|
|
||||||
int error = dx / 2;
|
|
||||||
const int direction = (from.y() < to.y()) ? 1 : -1;
|
|
||||||
|
|
||||||
for (; from.x() < to.x(); ++from.x()) {
|
|
||||||
if (large_delta_y) {
|
|
||||||
result->ApplyLookupTable(Eigen::Array2i(from.y(), from.x()), miss_table);
|
|
||||||
} else {
|
|
||||||
result->ApplyLookupTable(from, miss_table);
|
|
||||||
}
|
|
||||||
error -= dy;
|
|
||||||
if (error < 0) {
|
|
||||||
from.y() += direction;
|
|
||||||
error += dx;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void InsertSegmentsIntoProbabilityGrid(const std::vector<RaySegment>& segments,
|
|
||||||
const std::vector<uint16>& hit_table,
|
|
||||||
const std::vector<uint16>& miss_table,
|
|
||||||
mapping_2d::ProbabilityGrid* result) {
|
|
||||||
result->StartUpdate();
|
|
||||||
if (segments.empty()) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
Eigen::Vector2f min = segments.front().from;
|
|
||||||
Eigen::Vector2f max = min;
|
|
||||||
for (const RaySegment& segment : segments) {
|
|
||||||
min = min.cwiseMin(segment.from);
|
|
||||||
min = min.cwiseMin(segment.to);
|
|
||||||
max = max.cwiseMax(segment.from);
|
|
||||||
max = max.cwiseMax(segment.to);
|
|
||||||
}
|
|
||||||
const float padding = 10. * result->limits().resolution();
|
|
||||||
max += Eigen::Vector2f(padding, padding);
|
|
||||||
min -= Eigen::Vector2f(padding, padding);
|
|
||||||
result->GrowLimits(min.x(), min.y());
|
|
||||||
result->GrowLimits(max.x(), max.y());
|
|
||||||
|
|
||||||
for (const RaySegment& segment : segments) {
|
|
||||||
if (segment.hit) {
|
|
||||||
result->ApplyLookupTable(result->limits().GetXYIndexOfCellContainingPoint(
|
|
||||||
segment.to.x(), segment.to.y()),
|
|
||||||
hit_table);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
for (const RaySegment& segment : segments) {
|
|
||||||
UpdateFreeSpaceFromSegment(segment, miss_table, result);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Filters 'range_data', retaining only the returns that have no more than
|
// Filters 'range_data', retaining only the returns that have no more than
|
||||||
// 'max_range' distance from the origin. Removes misses and reflectivity
|
// 'max_range' distance from the origin. Removes misses and reflectivity
|
||||||
// information.
|
// information.
|
||||||
|
|
@ -289,20 +148,6 @@ string ComputePixelValues(
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
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::vector<RaySegment> segments;
|
|
||||||
GenerateSegmentForSlice(
|
|
||||||
range_data, pose,
|
|
||||||
transform::Rigid3f::Translation(-slice_z * Eigen::Vector3f::UnitZ()),
|
|
||||||
&segments);
|
|
||||||
InsertSegmentsIntoProbabilityGrid(segments, range_data_inserter.hit_table(),
|
|
||||||
range_data_inserter.miss_table(), result);
|
|
||||||
}
|
|
||||||
|
|
||||||
proto::SubmapsOptions CreateSubmapsOptions(
|
proto::SubmapsOptions CreateSubmapsOptions(
|
||||||
common::LuaParameterDictionary* parameter_dictionary) {
|
common::LuaParameterDictionary* parameter_dictionary) {
|
||||||
proto::SubmapsOptions options;
|
proto::SubmapsOptions options;
|
||||||
|
|
|
||||||
|
|
@ -27,7 +27,6 @@
|
||||||
#include "cartographer/mapping/proto/serialization.pb.h"
|
#include "cartographer/mapping/proto/serialization.pb.h"
|
||||||
#include "cartographer/mapping/proto/submap_visualization.pb.h"
|
#include "cartographer/mapping/proto/submap_visualization.pb.h"
|
||||||
#include "cartographer/mapping/submaps.h"
|
#include "cartographer/mapping/submaps.h"
|
||||||
#include "cartographer/mapping_2d/probability_grid.h"
|
|
||||||
#include "cartographer/mapping_2d/range_data_inserter.h"
|
#include "cartographer/mapping_2d/range_data_inserter.h"
|
||||||
#include "cartographer/mapping_3d/hybrid_grid.h"
|
#include "cartographer/mapping_3d/hybrid_grid.h"
|
||||||
#include "cartographer/mapping_3d/proto/submaps_options.pb.h"
|
#include "cartographer/mapping_3d/proto/submaps_options.pb.h"
|
||||||
|
|
@ -39,12 +38,6 @@
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping_3d {
|
namespace mapping_3d {
|
||||||
|
|
||||||
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);
|
|
||||||
|
|
||||||
proto::SubmapsOptions CreateSubmapsOptions(
|
proto::SubmapsOptions CreateSubmapsOptions(
|
||||||
common::LuaParameterDictionary* parameter_dictionary);
|
common::LuaParameterDictionary* parameter_dictionary);
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue