12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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=wohe
master
Holger Rapp 2017-07-05 13:17:20 +02:00 committed by Wolfgang Hess
parent 1e6723e214
commit 611f244d7a
2 changed files with 0 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

155
cartographer/mapping_3d/submaps.cc
View File

@ -28,14 +28,6 @@ namespace mapping_3d {
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 {
int min_z = INT_MAX;
int max_z = INT_MIN;
@ -44,139 +36,6 @@ struct PixelData {
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
// 'max_range' distance from the origin. Removes misses and reflectivity
// information.
@ -289,20 +148,6 @@ string ComputePixelValues(
} // 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(
common::LuaParameterDictionary* parameter_dictionary) {
proto::SubmapsOptions options;

7
cartographer/mapping_3d/submaps.h
View File

@ -27,7 +27,6 @@
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping_2d/probability_grid.h"
#include "cartographer/mapping_2d/range_data_inserter.h"
#include "cartographer/mapping_3d/hybrid_grid.h"
#include "cartographer/mapping_3d/proto/submaps_options.pb.h"
@ -39,12 +38,6 @@
namespace cartographer {
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(
common::LuaParameterDictionary* parameter_dictionary);