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

Move SubmapToProto from Submaps -> Submap. (#332) 

PAIR=wohe
master
Holger Rapp 2017-06-12 16:27:51 +02:00 committed by GitHub
parent 521666ce55
commit 2d75f4ef56
7 changed files with 162 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping/map_builder.cc
View File

@ -123,10 +123,10 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
" submaps in this trajectory.";
}
Submaps* const submaps = trajectory_builders_.at(trajectory_id)->submaps();
response->set_submap_version(submaps->Get(submap_index)->num_range_data);
submaps->SubmapToProto(submap_index, submap_transforms[submap_index],
response);
const Submap* const submap =
trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index);
response->set_submap_version(submap->num_range_data);
submap->ToResponseProto(submap_transforms[submap_index], response);
return "";
}

6
cartographer/mapping/sparse_pose_graph.cc
View File

@ -73,8 +73,10 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
old_node_index != single_trajectory_nodes.size(); ++old_node_index) {
const auto& node = single_trajectory_nodes[old_node_index];
if (!node.trimmed()) {
node_id_remapping[NodeId{trajectory_id, old_node_index}] =
NodeId{trajectory_id, trajectory_proto->node_size()};
node_id_remapping[NodeId{static_cast<int>(trajectory_id),
static_cast<int>(old_node_index)}] =
NodeId{static_cast<int>(trajectory_id),
static_cast<int>(trajectory_proto->node_size())};
auto* node_proto = trajectory_proto->add_node();
node_proto->set_timestamp(
common::ToUniversal(node.constant_data->time));

12
cartographer/mapping/submaps.h
View File

@ -56,8 +56,10 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
// track of how many range data were inserted into it, and sets the
// 'finished_probability_grid' to be used for loop closing once the map no
// longer changes.
// TODO(hrapp): This should be a class now.
struct Submap {
Submap(const transform::Rigid3d& local_pose) : local_pose(local_pose) {}
virtual ~Submap() {}
// Local SLAM pose of this submap.
const transform::Rigid3d local_pose;
@ -69,6 +71,11 @@ struct Submap {
// change anymore. Otherwise, this is nullptr and the next call to
// InsertRangeData() will change the submap.
const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr;
// Fills data into the 'response'.
virtual void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
proto::SubmapQuery::Response* response) const = 0;
};
// Submaps is a sequence of maps to which scans are matched and into which scans
@ -105,11 +112,6 @@ class Submaps {
// Returns the number of Submaps.
virtual int size() const = 0;
// Fills data about the Submap with 'index' into the 'response'.
virtual void SubmapToProto(int index,
const transform::Rigid3d& global_submap_pose,
proto::SubmapQuery::Response* response) = 0;
};
} // namespace mapping

11
cartographer/mapping_2d/submaps.cc
View File

@ -105,8 +105,9 @@ Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
probability_grid(limits) {}
void Submap::AddProbabilityGridToResponse(
mapping::proto::SubmapQuery::Response* response) {
void Submap::ToResponseProto(
const transform::Rigid3d&,
mapping::proto::SubmapQuery::Response* const response) const {
Eigen::Array2i offset;
CellLimits limits;
probability_grid.ComputeCroppedLimits(&offset, &limits);
@ -177,12 +178,6 @@ const Submap* Submaps::Get(int index) const {
int Submaps::size() const { return submaps_.size(); }
void Submaps::SubmapToProto(
const int index, const transform::Rigid3d&,
mapping::proto::SubmapQuery::Response* const response) {
submaps_.at(index)->AddProbabilityGridToResponse(response);
}
void Submaps::FinishSubmap(int index) {
// Crop the finished Submap before inserting a new Submap to reduce peak
// memory usage a bit.

7
cartographer/mapping_2d/submaps.h
View File

@ -46,8 +46,9 @@ struct Submap : public mapping::Submap {
ProbabilityGrid probability_grid;
void AddProbabilityGridToResponse(
mapping::proto::SubmapQuery::Response* response);
void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override;
};
// A container of Submaps.
@ -60,8 +61,6 @@ class Submaps : public mapping::Submaps {
const Submap* Get(int index) const override;
int size() const override;
void SubmapToProto(int index, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) override;
// Inserts 'range_data' into the Submap collection.
void InsertRangeData(const sensor::RangeData& range_data);

261
cartographer/mapping_3d/submaps.cc
View File

@ -36,6 +36,14 @@ struct RaySegment {
bool hit; // Whether there is a hit at 'to'.
};
struct PixelData {
int min_z = INT_MAX;
int max_z = INT_MIN;
int count = 0;
float probability_sum = 0.f;
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,
@ -183,6 +191,102 @@ sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data,
return result;
}
std::vector<PixelData> AccumulatePixelData(
const int width, const int height, const Eigen::Array2i& min_index,
const Eigen::Array2i& max_index,
const std::vector<Eigen::Array4i>& voxel_indices_and_probabilities) {
std::vector<PixelData> accumulated_pixel_data(width * height);
for (const Eigen::Array4i& voxel_index_and_probability :
voxel_indices_and_probabilities) {
const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
if ((pixel_index < min_index).any() || (pixel_index > max_index).any()) {
// Out of bounds. This could happen because of floating point inaccuracy.
continue;
}
const int x = max_index.x() - pixel_index[0];
const int y = max_index.y() - pixel_index[1];
PixelData& pixel = accumulated_pixel_data[x * width + y];
++pixel.count;
pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]);
pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]);
const float probability =
mapping::ValueToProbability(voxel_index_and_probability[3]);
pixel.probability_sum += probability;
pixel.max_probability = std::max(pixel.max_probability, probability);
}
return accumulated_pixel_data;
}
// The first three entries of each returned value are a cell_index and the
// last is the corresponding probability value. We batch them together like
// this to only have one vector and have better cache locality.
std::vector<Eigen::Array4i> ExtractVoxelData(
const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
Eigen::Array2i* min_index, Eigen::Array2i* max_index) {
std::vector<Eigen::Array4i> voxel_indices_and_probabilities;
const float resolution_inverse = 1. / hybrid_grid.resolution();
constexpr double kXrayObstructedCellProbabilityLimit = 0.501;
for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
const uint16 probability_value = it.GetValue();
const float probability = mapping::ValueToProbability(probability_value);
if (probability < kXrayObstructedCellProbabilityLimit) {
// We ignore non-obstructed cells.
continue;
}
const Eigen::Vector3f cell_center_submap =
hybrid_grid.GetCenterOfCell(it.GetCellIndex());
const Eigen::Vector3f cell_center_global = transform * cell_center_submap;
const Eigen::Array4i voxel_index_and_probability(
common::RoundToInt(cell_center_global.x() * resolution_inverse),
common::RoundToInt(cell_center_global.y() * resolution_inverse),
common::RoundToInt(cell_center_global.z() * resolution_inverse),
probability_value);
voxel_indices_and_probabilities.push_back(voxel_index_and_probability);
const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
*min_index = min_index->cwiseMin(pixel_index);
*max_index = max_index->cwiseMax(pixel_index);
}
return voxel_indices_and_probabilities;
}
// Builds texture data containing interleaved value and alpha for the
// visualization from 'accumulated_pixel_data'.
string ComputePixelValues(
const std::vector<PixelData>& accumulated_pixel_data) {
string cell_data;
cell_data.reserve(2 * accumulated_pixel_data.size());
constexpr float kMinZDifference = 3.f;
constexpr float kFreeSpaceWeight = 0.15f;
for (const PixelData& pixel : accumulated_pixel_data) {
// TODO(whess): Take into account submap rotation.
// TODO(whess): Document the approach and make it more independent from the
// chosen resolution.
const float z_difference = pixel.count > 0 ? pixel.max_z - pixel.min_z : 0;
if (z_difference < kMinZDifference) {
cell_data.push_back(0); // value
cell_data.push_back(0); // alpha
continue;
}
const float free_space = std::max(z_difference - pixel.count, 0.f);
const float free_space_weight = kFreeSpaceWeight * free_space;
const float total_weight = pixel.count + free_space_weight;
const float free_space_probability = 1.f - pixel.max_probability;
const float average_probability = mapping::ClampProbability(
(pixel.probability_sum + free_space_probability * free_space_weight) /
total_weight);
const int delta =
128 - mapping::ProbabilityToLogOddsInteger(average_probability);
const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0;
cell_data.push_back(value); // value
cell_data.push_back((value || alpha) ? alpha : 1); // alpha
}
return cell_data;
}
} // namespace
void InsertIntoProbabilityGrid(
@ -222,6 +326,39 @@ Submap::Submap(const float high_resolution, const float low_resolution,
high_resolution_hybrid_grid(high_resolution),
low_resolution_hybrid_grid(low_resolution) {}
void Submap::ToResponseProto(
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* const response) const {
// Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
// in the global map frame.
const float resolution = high_resolution_hybrid_grid.resolution();
response->set_resolution(resolution);
// Compute a bounding box for the texture.
Eigen::Array2i min_index(INT_MAX, INT_MAX);
Eigen::Array2i max_index(INT_MIN, INT_MIN);
const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
ExtractVoxelData(high_resolution_hybrid_grid,
global_submap_pose.cast<float>(), &min_index,
&max_index);
const int width = max_index.y() - min_index.y() + 1;
const int height = max_index.x() - min_index.x() + 1;
response->set_width(width);
response->set_height(height);
const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
width, height, min_index, max_index, voxel_indices_and_probabilities);
const string cell_data = ComputePixelValues(accumulated_pixel_data);
common::FastGzipString(cell_data, response->mutable_cells());
*response->mutable_slice_pose() = transform::ToProto(
global_submap_pose.inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(
max_index.x() * resolution, max_index.y() * resolution,
global_submap_pose.translation().z())));
}
Submaps::Submaps(const proto::SubmapsOptions& options)
: options_(options),
range_data_inserter_(options.range_data_inserter_options()) {
@ -241,39 +378,6 @@ const Submap* Submaps::Get(int index) const {
int Submaps::size() const { return submaps_.size(); }
void Submaps::SubmapToProto(
int index, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* const response) {
// Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
// in the global map frame.
const HybridGrid& hybrid_grid = Get(index)->high_resolution_hybrid_grid;
response->set_resolution(hybrid_grid.resolution());
// Compute a bounding box for the texture.
Eigen::Array2i min_index(INT_MAX, INT_MAX);
Eigen::Array2i max_index(INT_MIN, INT_MIN);
const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
ExtractVoxelData(hybrid_grid, global_submap_pose.cast<float>(),
&min_index, &max_index);
const int width = max_index.y() - min_index.y() + 1;
const int height = max_index.x() - min_index.x() + 1;
response->set_width(width);
response->set_height(height);
const std::vector<PixelData> accumulated_pixel_data = AccumulatePixelData(
width, height, min_index, max_index, voxel_indices_and_probabilities);
const string cell_data = ComputePixelValues(accumulated_pixel_data);
common::FastGzipString(cell_data, response->mutable_cells());
*response->mutable_slice_pose() =
transform::ToProto(global_submap_pose.inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(
max_index.x() * hybrid_grid.resolution(),
max_index.y() * hybrid_grid.resolution(),
global_submap_pose.translation().z())));
}
void Submaps::InsertRangeData(const sensor::RangeData& range_data,
const Eigen::Quaterniond& gravity_alignment) {
for (const int index : insertion_indices()) {
@ -306,96 +410,5 @@ void Submaps::AddSubmap(const transform::Rigid3d& local_pose) {
LOG(INFO) << "Added submap " << size();
}
std::vector<Submaps::PixelData> Submaps::AccumulatePixelData(
const int width, const int height, const Eigen::Array2i& min_index,
const Eigen::Array2i& max_index,
const std::vector<Eigen::Array4i>& voxel_indices_and_probabilities) const {
std::vector<PixelData> accumulated_pixel_data(width * height);
for (const Eigen::Array4i& voxel_index_and_probability :
voxel_indices_and_probabilities) {
const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
if ((pixel_index < min_index).any() || (pixel_index > max_index).any()) {
// Out of bounds. This could happen because of floating point inaccuracy.
continue;
}
const int x = max_index.x() - pixel_index[0];
const int y = max_index.y() - pixel_index[1];
PixelData& pixel = accumulated_pixel_data[x * width + y];
++pixel.count;
pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]);
pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]);
const float probability =
mapping::ValueToProbability(voxel_index_and_probability[3]);
pixel.probability_sum += probability;
pixel.max_probability = std::max(pixel.max_probability, probability);
}
return accumulated_pixel_data;
}
std::vector<Eigen::Array4i> Submaps::ExtractVoxelData(
const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
Eigen::Array2i* min_index, Eigen::Array2i* max_index) const {
std::vector<Eigen::Array4i> voxel_indices_and_probabilities;
const float resolution_inverse = 1. / hybrid_grid.resolution();
constexpr double kXrayObstructedCellProbabilityLimit = 0.501;
for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
const uint16 probability_value = it.GetValue();
const float probability = mapping::ValueToProbability(probability_value);
if (probability < kXrayObstructedCellProbabilityLimit) {
// We ignore non-obstructed cells.
continue;
}
const Eigen::Vector3f cell_center_submap =
hybrid_grid.GetCenterOfCell(it.GetCellIndex());
const Eigen::Vector3f cell_center_global = transform * cell_center_submap;
const Eigen::Array4i voxel_index_and_probability(
common::RoundToInt(cell_center_global.x() * resolution_inverse),
common::RoundToInt(cell_center_global.y() * resolution_inverse),
common::RoundToInt(cell_center_global.z() * resolution_inverse),
probability_value);
voxel_indices_and_probabilities.push_back(voxel_index_and_probability);
const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>();
*min_index = min_index->cwiseMin(pixel_index);
*max_index = max_index->cwiseMax(pixel_index);
}
return voxel_indices_and_probabilities;
}
string Submaps::ComputePixelValues(
const std::vector<Submaps::PixelData>& accumulated_pixel_data) const {
string cell_data;
cell_data.reserve(2 * accumulated_pixel_data.size());
constexpr float kMinZDifference = 3.f;
constexpr float kFreeSpaceWeight = 0.15f;
for (const PixelData& pixel : accumulated_pixel_data) {
// TODO(whess): Take into account submap rotation.
// TODO(whess): Document the approach and make it more independent from the
// chosen resolution.
const float z_difference = pixel.count > 0 ? pixel.max_z - pixel.min_z : 0;
if (z_difference < kMinZDifference) {
cell_data.push_back(0); // value
cell_data.push_back(0); // alpha
continue;
}
const float free_space = std::max(z_difference - pixel.count, 0.f);
const float free_space_weight = kFreeSpaceWeight * free_space;
const float total_weight = pixel.count + free_space_weight;
const float free_space_probability = 1.f - pixel.max_probability;
const float average_probability = mapping::ClampProbability(
(pixel.probability_sum + free_space_probability * free_space_weight) /
total_weight);
const int delta =
128 - mapping::ProbabilityToLogOddsInteger(average_probability);
const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0;
cell_data.push_back(value); // value
cell_data.push_back((value || alpha) ? alpha : 1); // alpha
}
return cell_data;
}
} // namespace mapping_3d
} // namespace cartographer

29
cartographer/mapping_3d/submaps.h
View File

@ -54,6 +54,10 @@ struct Submap : public mapping::Submap {
HybridGrid high_resolution_hybrid_grid;
HybridGrid low_resolution_hybrid_grid;
bool finished = false;
void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override;
};
// A container of Submaps.
@ -66,8 +70,6 @@ class Submaps : public mapping::Submaps {
const Submap* Get(int index) const override;
int size() const override;
void SubmapToProto(int index, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) override;
// Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
// used for the orientation of new submaps so that the z axis approximately
@ -76,31 +78,8 @@ class Submaps : public mapping::Submaps {
const Eigen::Quaterniond& gravity_alignment);
private:
struct PixelData {
int min_z = INT_MAX;
int max_z = INT_MIN;
int count = 0;
float probability_sum = 0.f;
float max_probability = 0.5f;
};
void AddSubmap(const transform::Rigid3d& local_pose);
std::vector<PixelData> AccumulatePixelData(
const int width, const int height, const Eigen::Array2i& min_index,
const Eigen::Array2i& max_index,
const std::vector<Eigen::Array4i>& voxel_indices_and_probabilities) const;
// The first three entries of each returned value are a cell_index and the
// last is the corresponding probability value. We batch them together like
// this to only have one vector and have better cache locality.
std::vector<Eigen::Array4i> ExtractVoxelData(
const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
Eigen::Array2i* min_index, Eigen::Array2i* max_index) const;
// Builds texture data containing interleaved value and alpha for the
// visualization from 'accumulated_pixel_data'.
string ComputePixelValues(
const std::vector<PixelData>& accumulated_pixel_data) const;
const proto::SubmapsOptions options_;
// 'submaps_' contains pointers, so that resizing the vector does not