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

Move AddProbabilityGridToResponse to the Submap object. (#318) 

Working towards a thread-safe Submap object. Related to #283.

PAIR=wohe
master
Holger Rapp 2017-06-07 15:13:56 +02:00 committed by GitHub
parent b3b9735a4b
commit c1d4d08a1d
22 changed files with 73 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/collated_trajectory_builder.cc
View File

@ -46,7 +46,7 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
const Submaps* CollatedTrajectoryBuilder::submaps() const {
Submaps* CollatedTrajectoryBuilder::submaps() {
return wrapped_trajectory_builder_->submaps();
}

2
cartographer/mapping/collated_trajectory_builder.h
View File

@ -49,7 +49,7 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
delete;
const Submaps* submaps() const override;
Submaps* submaps() override;
const PoseEstimate& pose_estimate() const override;
void AddSensorData(const string& sensor_id,

2
cartographer/mapping/global_trajectory_builder_interface.h
View File

@ -46,7 +46,7 @@ class GlobalTrajectoryBuilderInterface {
GlobalTrajectoryBuilderInterface& operator=(
const GlobalTrajectoryBuilderInterface&) = delete;
virtual const Submaps* submaps() const = 0;
virtual Submaps* submaps() = 0;
virtual const PoseEstimate& pose_estimate() const = 0;
virtual void AddRangefinderData(common::Time time,

3
cartographer/mapping/map_builder.cc
View File

@ -123,8 +123,7 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
" submaps in this trajectory.";
}
const Submaps* const submaps =
trajectory_builders_.at(trajectory_id)->submaps();
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);

47
cartographer/mapping/submaps.cc
View File

@ -24,8 +24,6 @@
namespace cartographer {
namespace mapping {
constexpr uint8 Submaps::kUnknownLogOdds;
Submaps::Submaps() {}
Submaps::~Submaps() {}
@ -44,50 +42,5 @@ std::vector<int> Submaps::insertion_indices() const {
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 cartographer

10
cartographer/mapping/submaps.h
View File

@ -85,8 +85,6 @@ struct Submap {
// a "new" submap gets inserted.
class Submaps {
public:
static constexpr uint8 kUnknownLogOdds = 0;
Submaps();
virtual ~Submaps();
@ -111,13 +109,7 @@ class Submaps {
// 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) const = 0;
protected:
static void AddProbabilityGridToResponse(
const transform::Rigid3d& local_submap_pose,
const mapping_2d::ProbabilityGrid& probability_grid,
proto::SubmapQuery::Response* response);
proto::SubmapQuery::Response* response) = 0;
};
} // namespace mapping

2
cartographer/mapping/trajectory_builder.h
View File

@ -59,7 +59,7 @@ class TrajectoryBuilder {
TrajectoryBuilder(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 void AddSensorData(const string& sensor_id,

2
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -28,7 +28,7 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
const Submaps* GlobalTrajectoryBuilder::submaps() const {
Submaps* GlobalTrajectoryBuilder::submaps() {
return local_trajectory_builder_.submaps();
}

2
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -35,7 +35,7 @@ class GlobalTrajectoryBuilder
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
const Submaps* submaps() const override;
Submaps* submaps() override;
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
const override;

2
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -36,7 +36,7 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
Submaps* LocalTrajectoryBuilder::submaps() { return &submaps_; }
sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
const transform::Rigid3f& tracking_to_tracking_2d,

2
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -63,7 +63,7 @@ class LocalTrajectoryBuilder {
const Eigen::Vector3d& angular_velocity);
void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
const Submaps* submaps() const;
Submaps* submaps();
private:
sensor::RangeData TransformAndFilterRangeData(

55
cartographer/mapping_2d/submaps.cc
View File

@ -36,13 +36,12 @@ namespace {
void WriteDebugImage(const string& filename,
const ProbabilityGrid& probability_grid) {
constexpr int kUnknown = 128;
const mapping_2d::CellLimits& cell_limits =
probability_grid.limits().cell_limits();
const CellLimits& cell_limits = probability_grid.limits().cell_limits();
const int width = cell_limits.num_x_cells;
const int height = cell_limits.num_y_cells;
std::vector<uint8_t> rgb;
for (const Eigen::Array2i& xy_index : mapping_2d::XYIndexRangeIterator(
probability_grid.limits().cell_limits())) {
for (const Eigen::Array2i& xy_index :
XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
CHECK(probability_grid.limits().Contains(xy_index));
const uint8_t value =
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.))),
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)
: options_(options),
range_data_inserter_(options.range_data_inserter_options()) {
@ -137,9 +179,8 @@ int Submaps::size() const { return submaps_.size(); }
void Submaps::SubmapToProto(
const int index, const transform::Rigid3d&,
mapping::proto::SubmapQuery::Response* const response) const {
AddProbabilityGridToResponse(Get(index)->local_pose,
Get(index)->probability_grid, response);
mapping::proto::SubmapQuery::Response* const response) {
submaps_.at(index)->AddProbabilityGridToResponse(response);
}
void Submaps::FinishSubmap(int index) {

8
cartographer/mapping_2d/submaps.h
View File

@ -45,6 +45,9 @@ struct Submap : public mapping::Submap {
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
ProbabilityGrid probability_grid;
void AddProbabilityGridToResponse(
mapping::proto::SubmapQuery::Response* response);
};
// A container of Submaps.
@ -57,9 +60,8 @@ 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) 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);

2
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -30,7 +30,7 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
const mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() const {
mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() {
return local_trajectory_builder_->submaps();
}

2
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -38,7 +38,7 @@ class GlobalTrajectoryBuilder
GlobalTrajectoryBuilder(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,
const Eigen::Vector3d& angular_velocity) override;
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,

2
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -46,7 +46,7 @@ KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder(
KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {}
const mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() const {
mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() {
return submaps_.get();
}

2
cartographer/mapping_3d/kalman_local_trajectory_builder.h
View File

@ -53,7 +53,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
const sensor::PointCloud& ranges) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
const mapping_3d::Submaps* submaps() const override;
mapping_3d::Submaps* submaps() override;
const PoseEstimate& pose_estimate() const override;
private:

2
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -57,7 +57,7 @@ class LocalTrajectoryBuilderInterface {
virtual void AddOdometerData(common::Time time,
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;
protected:

2
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -112,7 +112,7 @@ OptimizingLocalTrajectoryBuilder::OptimizingLocalTrajectoryBuilder(
OptimizingLocalTrajectoryBuilder::~OptimizingLocalTrajectoryBuilder() {}
const mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() const {
mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() {
return submaps_.get();
}

2
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

@ -56,7 +56,7 @@ class OptimizingLocalTrajectoryBuilder
const sensor::PointCloud& ranges) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
const mapping_3d::Submaps* submaps() const override;
mapping_3d::Submaps* submaps() override;
const PoseEstimate& pose_estimate() const override;
private:

2
cartographer/mapping_3d/submaps.cc
View File

@ -243,7 +243,7 @@ int Submaps::size() const { return submaps_.size(); }
void Submaps::SubmapToProto(
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
// in the global map frame.
const HybridGrid& hybrid_grid = Get(index)->high_resolution_hybrid_grid;

5
cartographer/mapping_3d/submaps.h
View File

@ -66,9 +66,8 @@ 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) 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