Move AddProbabilityGridToResponse to the Submap object. (#318)
Working towards a thread-safe Submap object. Related to #283. PAIR=wohemaster
parent
b3b9735a4b
commit
c1d4d08a1d
|
@ -46,7 +46,7 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
|
||||||
|
|
||||||
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
|
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
|
||||||
|
|
||||||
const Submaps* CollatedTrajectoryBuilder::submaps() const {
|
Submaps* CollatedTrajectoryBuilder::submaps() {
|
||||||
return wrapped_trajectory_builder_->submaps();
|
return wrapped_trajectory_builder_->submaps();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -49,7 +49,7 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
|
||||||
CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
|
CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
|
||||||
delete;
|
delete;
|
||||||
|
|
||||||
const Submaps* submaps() const override;
|
Submaps* submaps() override;
|
||||||
const PoseEstimate& pose_estimate() const override;
|
const PoseEstimate& pose_estimate() const override;
|
||||||
|
|
||||||
void AddSensorData(const string& sensor_id,
|
void AddSensorData(const string& sensor_id,
|
||||||
|
|
|
@ -46,7 +46,7 @@ class GlobalTrajectoryBuilderInterface {
|
||||||
GlobalTrajectoryBuilderInterface& operator=(
|
GlobalTrajectoryBuilderInterface& operator=(
|
||||||
const GlobalTrajectoryBuilderInterface&) = delete;
|
const GlobalTrajectoryBuilderInterface&) = delete;
|
||||||
|
|
||||||
virtual const Submaps* submaps() const = 0;
|
virtual Submaps* submaps() = 0;
|
||||||
virtual const PoseEstimate& pose_estimate() const = 0;
|
virtual const PoseEstimate& pose_estimate() const = 0;
|
||||||
|
|
||||||
virtual void AddRangefinderData(common::Time time,
|
virtual void AddRangefinderData(common::Time time,
|
||||||
|
|
|
@ -123,8 +123,7 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
|
||||||
" submaps in this trajectory.";
|
" submaps in this trajectory.";
|
||||||
}
|
}
|
||||||
|
|
||||||
const Submaps* const submaps =
|
Submaps* const submaps = trajectory_builders_.at(trajectory_id)->submaps();
|
||||||
trajectory_builders_.at(trajectory_id)->submaps();
|
|
||||||
response->set_submap_version(submaps->Get(submap_index)->num_range_data);
|
response->set_submap_version(submaps->Get(submap_index)->num_range_data);
|
||||||
submaps->SubmapToProto(submap_index, submap_transforms[submap_index],
|
submaps->SubmapToProto(submap_index, submap_transforms[submap_index],
|
||||||
response);
|
response);
|
||||||
|
|
|
@ -24,8 +24,6 @@
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping {
|
namespace mapping {
|
||||||
|
|
||||||
constexpr uint8 Submaps::kUnknownLogOdds;
|
|
||||||
|
|
||||||
Submaps::Submaps() {}
|
Submaps::Submaps() {}
|
||||||
|
|
||||||
Submaps::~Submaps() {}
|
Submaps::~Submaps() {}
|
||||||
|
@ -44,50 +42,5 @@ std::vector<int> Submaps::insertion_indices() const {
|
||||||
return {size() - 1};
|
return {size() - 1};
|
||||||
}
|
}
|
||||||
|
|
||||||
void Submaps::AddProbabilityGridToResponse(
|
|
||||||
const transform::Rigid3d& local_submap_pose,
|
|
||||||
const mapping_2d::ProbabilityGrid& probability_grid,
|
|
||||||
proto::SubmapQuery::Response* response) {
|
|
||||||
Eigen::Array2i offset;
|
|
||||||
mapping_2d::CellLimits limits;
|
|
||||||
probability_grid.ComputeCroppedLimits(&offset, &limits);
|
|
||||||
|
|
||||||
string cells;
|
|
||||||
for (const Eigen::Array2i& xy_index :
|
|
||||||
mapping_2d::XYIndexRangeIterator(limits)) {
|
|
||||||
if (probability_grid.IsKnown(xy_index + offset)) {
|
|
||||||
// We would like to add 'delta' but this is not possible using a value and
|
|
||||||
// alpha. We use premultiplied alpha, so when 'delta' is positive we can
|
|
||||||
// add it by setting 'alpha' to zero. If it is negative, we set 'value' to
|
|
||||||
// zero, and use 'alpha' to subtract. This is only correct when the pixel
|
|
||||||
// is currently white, so walls will look too gray. This should be hard to
|
|
||||||
// detect visually for the user, though.
|
|
||||||
const int delta =
|
|
||||||
128 - ProbabilityToLogOddsInteger(
|
|
||||||
probability_grid.GetProbability(xy_index + offset));
|
|
||||||
const uint8 alpha = delta > 0 ? 0 : -delta;
|
|
||||||
const uint8 value = delta > 0 ? delta : 0;
|
|
||||||
cells.push_back(value);
|
|
||||||
cells.push_back((value || alpha) ? alpha : 1);
|
|
||||||
} else {
|
|
||||||
cells.push_back(static_cast<uint8>(kUnknownLogOdds)); // value
|
|
||||||
cells.push_back(0); // alpha
|
|
||||||
}
|
|
||||||
}
|
|
||||||
common::FastGzipString(cells, response->mutable_cells());
|
|
||||||
|
|
||||||
response->set_width(limits.num_x_cells);
|
|
||||||
response->set_height(limits.num_y_cells);
|
|
||||||
const double resolution = probability_grid.limits().resolution();
|
|
||||||
response->set_resolution(resolution);
|
|
||||||
const double max_x =
|
|
||||||
probability_grid.limits().max().x() - resolution * offset.y();
|
|
||||||
const double max_y =
|
|
||||||
probability_grid.limits().max().y() - resolution * offset.x();
|
|
||||||
*response->mutable_slice_pose() = transform::ToProto(
|
|
||||||
local_submap_pose.inverse() *
|
|
||||||
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -85,8 +85,6 @@ struct Submap {
|
||||||
// a "new" submap gets inserted.
|
// a "new" submap gets inserted.
|
||||||
class Submaps {
|
class Submaps {
|
||||||
public:
|
public:
|
||||||
static constexpr uint8 kUnknownLogOdds = 0;
|
|
||||||
|
|
||||||
Submaps();
|
Submaps();
|
||||||
virtual ~Submaps();
|
virtual ~Submaps();
|
||||||
|
|
||||||
|
@ -111,13 +109,7 @@ class Submaps {
|
||||||
// Fills data about the Submap with 'index' into the 'response'.
|
// Fills data about the Submap with 'index' into the 'response'.
|
||||||
virtual void SubmapToProto(int index,
|
virtual void SubmapToProto(int index,
|
||||||
const transform::Rigid3d& global_submap_pose,
|
const transform::Rigid3d& global_submap_pose,
|
||||||
proto::SubmapQuery::Response* response) const = 0;
|
proto::SubmapQuery::Response* response) = 0;
|
||||||
|
|
||||||
protected:
|
|
||||||
static void AddProbabilityGridToResponse(
|
|
||||||
const transform::Rigid3d& local_submap_pose,
|
|
||||||
const mapping_2d::ProbabilityGrid& probability_grid,
|
|
||||||
proto::SubmapQuery::Response* response);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
|
|
|
@ -59,7 +59,7 @@ class TrajectoryBuilder {
|
||||||
TrajectoryBuilder(const TrajectoryBuilder&) = delete;
|
TrajectoryBuilder(const TrajectoryBuilder&) = delete;
|
||||||
TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
|
TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
|
||||||
|
|
||||||
virtual const Submaps* submaps() const = 0;
|
virtual Submaps* submaps() = 0;
|
||||||
virtual const PoseEstimate& pose_estimate() const = 0;
|
virtual const PoseEstimate& pose_estimate() const = 0;
|
||||||
|
|
||||||
virtual void AddSensorData(const string& sensor_id,
|
virtual void AddSensorData(const string& sensor_id,
|
||||||
|
|
|
@ -28,7 +28,7 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
|
||||||
|
|
||||||
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
||||||
|
|
||||||
const Submaps* GlobalTrajectoryBuilder::submaps() const {
|
Submaps* GlobalTrajectoryBuilder::submaps() {
|
||||||
return local_trajectory_builder_.submaps();
|
return local_trajectory_builder_.submaps();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -35,7 +35,7 @@ class GlobalTrajectoryBuilder
|
||||||
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
||||||
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
||||||
|
|
||||||
const Submaps* submaps() const override;
|
Submaps* submaps() override;
|
||||||
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
|
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
|
||||||
const override;
|
const override;
|
||||||
|
|
||||||
|
|
|
@ -36,7 +36,7 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
|
||||||
|
|
||||||
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
|
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
|
||||||
|
|
||||||
const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
|
Submaps* LocalTrajectoryBuilder::submaps() { return &submaps_; }
|
||||||
|
|
||||||
sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
|
sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
|
||||||
const transform::Rigid3f& tracking_to_tracking_2d,
|
const transform::Rigid3f& tracking_to_tracking_2d,
|
||||||
|
|
|
@ -63,7 +63,7 @@ class LocalTrajectoryBuilder {
|
||||||
const Eigen::Vector3d& angular_velocity);
|
const Eigen::Vector3d& angular_velocity);
|
||||||
void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
|
void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
|
||||||
|
|
||||||
const Submaps* submaps() const;
|
Submaps* submaps();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
sensor::RangeData TransformAndFilterRangeData(
|
sensor::RangeData TransformAndFilterRangeData(
|
||||||
|
|
|
@ -36,13 +36,12 @@ namespace {
|
||||||
void WriteDebugImage(const string& filename,
|
void WriteDebugImage(const string& filename,
|
||||||
const ProbabilityGrid& probability_grid) {
|
const ProbabilityGrid& probability_grid) {
|
||||||
constexpr int kUnknown = 128;
|
constexpr int kUnknown = 128;
|
||||||
const mapping_2d::CellLimits& cell_limits =
|
const CellLimits& cell_limits = probability_grid.limits().cell_limits();
|
||||||
probability_grid.limits().cell_limits();
|
|
||||||
const int width = cell_limits.num_x_cells;
|
const int width = cell_limits.num_x_cells;
|
||||||
const int height = cell_limits.num_y_cells;
|
const int height = cell_limits.num_y_cells;
|
||||||
std::vector<uint8_t> rgb;
|
std::vector<uint8_t> rgb;
|
||||||
for (const Eigen::Array2i& xy_index : mapping_2d::XYIndexRangeIterator(
|
for (const Eigen::Array2i& xy_index :
|
||||||
probability_grid.limits().cell_limits())) {
|
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
|
||||||
CHECK(probability_grid.limits().Contains(xy_index));
|
CHECK(probability_grid.limits().Contains(xy_index));
|
||||||
const uint8_t value =
|
const uint8_t value =
|
||||||
probability_grid.IsKnown(xy_index)
|
probability_grid.IsKnown(xy_index)
|
||||||
|
@ -106,6 +105,49 @@ Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
|
||||||
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
|
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
|
||||||
probability_grid(limits) {}
|
probability_grid(limits) {}
|
||||||
|
|
||||||
|
void Submap::AddProbabilityGridToResponse(
|
||||||
|
mapping::proto::SubmapQuery::Response* response) {
|
||||||
|
Eigen::Array2i offset;
|
||||||
|
CellLimits limits;
|
||||||
|
probability_grid.ComputeCroppedLimits(&offset, &limits);
|
||||||
|
|
||||||
|
string cells;
|
||||||
|
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
|
||||||
|
if (probability_grid.IsKnown(xy_index + offset)) {
|
||||||
|
// We would like to add 'delta' but this is not possible using a value and
|
||||||
|
// alpha. We use premultiplied alpha, so when 'delta' is positive we can
|
||||||
|
// add it by setting 'alpha' to zero. If it is negative, we set 'value' to
|
||||||
|
// zero, and use 'alpha' to subtract. This is only correct when the pixel
|
||||||
|
// is currently white, so walls will look too gray. This should be hard to
|
||||||
|
// detect visually for the user, though.
|
||||||
|
const int delta =
|
||||||
|
128 - mapping::ProbabilityToLogOddsInteger(
|
||||||
|
probability_grid.GetProbability(xy_index + offset));
|
||||||
|
const uint8 alpha = delta > 0 ? 0 : -delta;
|
||||||
|
const uint8 value = delta > 0 ? delta : 0;
|
||||||
|
cells.push_back(value);
|
||||||
|
cells.push_back((value || alpha) ? alpha : 1);
|
||||||
|
} else {
|
||||||
|
constexpr uint8 kUnknownLogOdds = 0;
|
||||||
|
cells.push_back(static_cast<uint8>(kUnknownLogOdds)); // value
|
||||||
|
cells.push_back(0); // alpha
|
||||||
|
}
|
||||||
|
}
|
||||||
|
common::FastGzipString(cells, response->mutable_cells());
|
||||||
|
|
||||||
|
response->set_width(limits.num_x_cells);
|
||||||
|
response->set_height(limits.num_y_cells);
|
||||||
|
const double resolution = probability_grid.limits().resolution();
|
||||||
|
response->set_resolution(resolution);
|
||||||
|
const double max_x =
|
||||||
|
probability_grid.limits().max().x() - resolution * offset.y();
|
||||||
|
const double max_y =
|
||||||
|
probability_grid.limits().max().y() - resolution * offset.x();
|
||||||
|
*response->mutable_slice_pose() = transform::ToProto(
|
||||||
|
local_pose.inverse() *
|
||||||
|
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
|
||||||
|
}
|
||||||
|
|
||||||
Submaps::Submaps(const proto::SubmapsOptions& options)
|
Submaps::Submaps(const proto::SubmapsOptions& options)
|
||||||
: options_(options),
|
: options_(options),
|
||||||
range_data_inserter_(options.range_data_inserter_options()) {
|
range_data_inserter_(options.range_data_inserter_options()) {
|
||||||
|
@ -137,9 +179,8 @@ int Submaps::size() const { return submaps_.size(); }
|
||||||
|
|
||||||
void Submaps::SubmapToProto(
|
void Submaps::SubmapToProto(
|
||||||
const int index, const transform::Rigid3d&,
|
const int index, const transform::Rigid3d&,
|
||||||
mapping::proto::SubmapQuery::Response* const response) const {
|
mapping::proto::SubmapQuery::Response* const response) {
|
||||||
AddProbabilityGridToResponse(Get(index)->local_pose,
|
submaps_.at(index)->AddProbabilityGridToResponse(response);
|
||||||
Get(index)->probability_grid, response);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Submaps::FinishSubmap(int index) {
|
void Submaps::FinishSubmap(int index) {
|
||||||
|
|
|
@ -45,6 +45,9 @@ struct Submap : public mapping::Submap {
|
||||||
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
|
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
|
||||||
|
|
||||||
ProbabilityGrid probability_grid;
|
ProbabilityGrid probability_grid;
|
||||||
|
|
||||||
|
void AddProbabilityGridToResponse(
|
||||||
|
mapping::proto::SubmapQuery::Response* response);
|
||||||
};
|
};
|
||||||
|
|
||||||
// A container of Submaps.
|
// A container of Submaps.
|
||||||
|
@ -57,9 +60,8 @@ class Submaps : public mapping::Submaps {
|
||||||
|
|
||||||
const Submap* Get(int index) const override;
|
const Submap* Get(int index) const override;
|
||||||
int size() const override;
|
int size() const override;
|
||||||
void SubmapToProto(
|
void SubmapToProto(int index, const transform::Rigid3d& global_submap_pose,
|
||||||
int index, const transform::Rigid3d& global_submap_pose,
|
mapping::proto::SubmapQuery::Response* response) override;
|
||||||
mapping::proto::SubmapQuery::Response* response) const override;
|
|
||||||
|
|
||||||
// Inserts 'range_data' into the Submap collection.
|
// Inserts 'range_data' into the Submap collection.
|
||||||
void InsertRangeData(const sensor::RangeData& range_data);
|
void InsertRangeData(const sensor::RangeData& range_data);
|
||||||
|
|
|
@ -30,7 +30,7 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
|
||||||
|
|
||||||
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
||||||
|
|
||||||
const mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() const {
|
mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() {
|
||||||
return local_trajectory_builder_->submaps();
|
return local_trajectory_builder_->submaps();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -38,7 +38,7 @@ class GlobalTrajectoryBuilder
|
||||||
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
||||||
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
||||||
|
|
||||||
const mapping_3d::Submaps* submaps() const override;
|
mapping_3d::Submaps* submaps() override;
|
||||||
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
|
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||||
const Eigen::Vector3d& angular_velocity) override;
|
const Eigen::Vector3d& angular_velocity) override;
|
||||||
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
|
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
|
||||||
|
|
|
@ -46,7 +46,7 @@ KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder(
|
||||||
|
|
||||||
KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {}
|
KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {}
|
||||||
|
|
||||||
const mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() const {
|
mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() {
|
||||||
return submaps_.get();
|
return submaps_.get();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -53,7 +53,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
|
||||||
const sensor::PointCloud& ranges) override;
|
const sensor::PointCloud& ranges) override;
|
||||||
void AddOdometerData(common::Time time,
|
void AddOdometerData(common::Time time,
|
||||||
const transform::Rigid3d& pose) override;
|
const transform::Rigid3d& pose) override;
|
||||||
const mapping_3d::Submaps* submaps() const override;
|
mapping_3d::Submaps* submaps() override;
|
||||||
const PoseEstimate& pose_estimate() const override;
|
const PoseEstimate& pose_estimate() const override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -57,7 +57,7 @@ class LocalTrajectoryBuilderInterface {
|
||||||
virtual void AddOdometerData(common::Time time,
|
virtual void AddOdometerData(common::Time time,
|
||||||
const transform::Rigid3d& pose) = 0;
|
const transform::Rigid3d& pose) = 0;
|
||||||
|
|
||||||
virtual const mapping_3d::Submaps* submaps() const = 0;
|
virtual mapping_3d::Submaps* submaps() = 0;
|
||||||
virtual const PoseEstimate& pose_estimate() const = 0;
|
virtual const PoseEstimate& pose_estimate() const = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
|
@ -112,7 +112,7 @@ OptimizingLocalTrajectoryBuilder::OptimizingLocalTrajectoryBuilder(
|
||||||
|
|
||||||
OptimizingLocalTrajectoryBuilder::~OptimizingLocalTrajectoryBuilder() {}
|
OptimizingLocalTrajectoryBuilder::~OptimizingLocalTrajectoryBuilder() {}
|
||||||
|
|
||||||
const mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() const {
|
mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() {
|
||||||
return submaps_.get();
|
return submaps_.get();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -56,7 +56,7 @@ class OptimizingLocalTrajectoryBuilder
|
||||||
const sensor::PointCloud& ranges) override;
|
const sensor::PointCloud& ranges) override;
|
||||||
void AddOdometerData(common::Time time,
|
void AddOdometerData(common::Time time,
|
||||||
const transform::Rigid3d& pose) override;
|
const transform::Rigid3d& pose) override;
|
||||||
const mapping_3d::Submaps* submaps() const override;
|
mapping_3d::Submaps* submaps() override;
|
||||||
const PoseEstimate& pose_estimate() const override;
|
const PoseEstimate& pose_estimate() const override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -243,7 +243,7 @@ int Submaps::size() const { return submaps_.size(); }
|
||||||
|
|
||||||
void Submaps::SubmapToProto(
|
void Submaps::SubmapToProto(
|
||||||
int index, const transform::Rigid3d& global_submap_pose,
|
int index, const transform::Rigid3d& global_submap_pose,
|
||||||
mapping::proto::SubmapQuery::Response* const response) const {
|
mapping::proto::SubmapQuery::Response* const response) {
|
||||||
// Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
|
// Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
|
||||||
// in the global map frame.
|
// in the global map frame.
|
||||||
const HybridGrid& hybrid_grid = Get(index)->high_resolution_hybrid_grid;
|
const HybridGrid& hybrid_grid = Get(index)->high_resolution_hybrid_grid;
|
||||||
|
|
|
@ -66,9 +66,8 @@ class Submaps : public mapping::Submaps {
|
||||||
|
|
||||||
const Submap* Get(int index) const override;
|
const Submap* Get(int index) const override;
|
||||||
int size() const override;
|
int size() const override;
|
||||||
void SubmapToProto(
|
void SubmapToProto(int index, const transform::Rigid3d& global_submap_pose,
|
||||||
int index, const transform::Rigid3d& global_submap_pose,
|
mapping::proto::SubmapQuery::Response* response) override;
|
||||||
mapping::proto::SubmapQuery::Response* response) const override;
|
|
||||||
|
|
||||||
// Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
|
// Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
|
||||||
// used for the orientation of new submaps so that the z axis approximately
|
// used for the orientation of new submaps so that the z axis approximately
|
||||||
|
|
Loading…
Reference in New Issue