diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 645f64a..1bbe606 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -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 ""; } diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index b5a6dfe..583f380 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -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(trajectory_id), + static_cast(old_node_index)}] = + NodeId{static_cast(trajectory_id), + static_cast(trajectory_proto->node_size())}; auto* node_proto = trajectory_proto->add_node(); node_proto->set_timestamp( common::ToUniversal(node.constant_data->time)); diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index fe38bfb..b207ccb 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -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 diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index 96cee22..1e4b831 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -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. diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index 1004946..316218d 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -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); diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index 4515658..cea5811 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -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 AccumulatePixelData( + const int width, const int height, const Eigen::Array2i& min_index, + const Eigen::Array2i& max_index, + const std::vector& voxel_indices_and_probabilities) { + std::vector 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 ExtractVoxelData( + const HybridGrid& hybrid_grid, const transform::Rigid3f& transform, + Eigen::Array2i* min_index, Eigen::Array2i* max_index) { + std::vector 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& 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 voxel_indices_and_probabilities = + ExtractVoxelData(high_resolution_hybrid_grid, + global_submap_pose.cast(), &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 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 voxel_indices_and_probabilities = - ExtractVoxelData(hybrid_grid, global_submap_pose.cast(), - &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 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::AccumulatePixelData( - const int width, const int height, const Eigen::Array2i& min_index, - const Eigen::Array2i& max_index, - const std::vector& voxel_indices_and_probabilities) const { - std::vector 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 Submaps::ExtractVoxelData( - const HybridGrid& hybrid_grid, const transform::Rigid3f& transform, - Eigen::Array2i* min_index, Eigen::Array2i* max_index) const { - std::vector 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& 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 diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index f33648a..3d4a036 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -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 AccumulatePixelData( - const int width, const int height, const Eigen::Array2i& min_index, - const Eigen::Array2i& max_index, - const std::vector& 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 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& accumulated_pixel_data) const; - const proto::SubmapsOptions options_; // 'submaps_' contains pointers, so that resizing the vector does not