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

Unify 2D/3D range data in trajectory nodes. (#396) 

Since we only convert compressed range data to protos, we remove the
unused code to serialize (non-compressed) range data which should also
not be used in the future.
master
Wolfgang Hess 2017-07-07 17:07:04 +02:00 committed by GitHub
parent 5378ee2adc
commit 78bd37ec26
15 changed files with 53 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cartographer/mapping/map_builder.cc
View File

@ -175,13 +175,10 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
range_data_proto->mutable_node_id()->set_trajectory_id(trajectory_id);
range_data_proto->mutable_node_id()->set_node_index(node_index);
const auto& data = *node_data[trajectory_id][node_index].constant_data;
if (!data.range_data_2d.returns.empty()) {
*range_data_proto->mutable_range_data_2d() =
sensor::ToProto(sensor::Compress(data.range_data_2d));
} else {
*range_data_proto->mutable_range_data_3d() =
sensor::ToProto(data.range_data_3d);
}
*range_data_proto->mutable_range_data() =
sensor::ToProto(sensor::Compress(sensor::TransformRangeData(
sensor::Decompress(data.range_data),
data.tracking_to_pose.inverse().cast<float>())));
// TODO(whess): Only enable optionally? Resulting pbstream files will be
// a lot larger now.
writer->WriteProto(proto);

3
cartographer/mapping/proto/serialization.proto
View File

@ -28,8 +28,7 @@ message Submap {
message RangeData {
optional NodeId node_id = 1;
optional sensor.proto.CompressedRangeData range_data_2d = 2;
optional sensor.proto.CompressedRangeData range_data_3d = 3;
optional sensor.proto.CompressedRangeData range_data = 4;
}
message SerializedData {

7
cartographer/mapping/trajectory_node.h
View File

@ -32,11 +32,8 @@ struct TrajectoryNode {
struct Data {
common::Time time;
// Range data in 'pose' frame. Only used in the 2D case.
sensor::RangeData range_data_2d;
// Range data in 'pose' frame. Only used in the 3D case.
sensor::CompressedRangeData range_data_3d;
// Range data in 'pose' frame.
sensor::CompressedRangeData range_data;
// Transform from the 3D 'tracking' frame to the 'pose' frame of the range
// data, which contains roll, pitch and height for 2D. In 3D this is always

10
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -113,10 +113,8 @@ void SparsePoseGraph::AddScan(
trajectory_id,
mapping::TrajectoryNode{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
tracking_to_pose}),
mapping::TrajectoryNode::Data{time, Compress(range_data_in_pose),
tracking_to_pose}),
optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);
@ -178,7 +176,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
&trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
&trajectory_nodes_.at(node_id).constant_data->range_data.returns,
&trajectory_connectivity_);
} else {
const bool scan_and_submap_trajectories_connected =
@ -202,7 +200,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
.point_cloud_pose;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
&trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
&trajectory_nodes_.at(node_id).constant_data->range_data.returns,
initial_relative_pose);
}
}

20
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -61,7 +61,8 @@ ConstraintBuilder::~ConstraintBuilder() {
void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {
@ -75,10 +76,10 @@ void ConstraintBuilder::MaybeAddConstraint(
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint);
ComputeConstraint(
submap_id, submap, node_id, false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
compressed_point_cloud, initial_relative_pose, constraint);
FinishComputation(current_computation);
});
}
@ -86,7 +87,8 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* const compressed_point_cloud,
mapping::TrajectoryConnectivity* const trajectory_connectivity) {
common::MutexLocker locker(&mutex_);
constraints_.emplace_back();
@ -97,7 +99,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */
trajectory_connectivity, point_cloud,
trajectory_connectivity, compressed_point_cloud,
transform::Rigid2d::Identity(), constraint);
FinishComputation(current_computation);
});
@ -162,7 +164,7 @@ void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
const transform::Rigid2d initial_pose =
@ -170,7 +172,7 @@ void ConstraintBuilder::ComputeConstraint(
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id);
const sensor::PointCloud filtered_point_cloud =
adaptive_voxel_filter_.Filter(*point_cloud);
adaptive_voxel_filter_.Filter(compressed_point_cloud->Decompress());
// The 'constraint_transform' (submap i <- scan j) is computed from:
// - a 'filtered_point_cloud' in scan j,

36
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
View File

@ -72,27 +72,29 @@ class ConstraintBuilder {
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'point_cloud' for 'node_id'. The 'initial_pose' is
// relative to the 'submap'.
// 'submap_id', and the 'compressed_point_cloud' for 'node_id'. The
// 'initial_relative_pose' is relative to the 'submap'.
//
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddConstraint(const mapping::SubmapId& submap_id,
const Submap* submap, const mapping::NodeId& node_id,
const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose);
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* compressed_point_cloud,
const transform::Rigid2d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'point_cloud' for 'node_id'. This performs full-submap
// matching.
// 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
// This performs full-submap matching.
//
// The 'trajectory_connectivity' is updated if the full-submap match succeeds.
//
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* compressed_point_cloud,
mapping::TrajectoryConnectivity* trajectory_connectivity);
// Must be called after all computations related to one node have been added.
@ -131,16 +133,16 @@ class ConstraintBuilder {
const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore.
// If 'match_full_submap' is true, and global localization succeeds, will
// connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
// constraint, assuming 'submap' and 'compressed_point_cloud' do not change
// anymore. If 'match_full_submap' is true, and global localization succeeds,
// will connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
// 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud,
const sensor::CompressedPointCloud* compressed_point_cloud,
const transform::Rigid2d& initial_relative_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);

2
cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc
View File

@ -150,7 +150,7 @@ RotationalScanMatcher::RotationalScanMatcher(
for (const mapping::TrajectoryNode& node : nodes) {
AddValuesToHistogram(
GetValuesForHistogram(sensor::TransformPointCloud(
node.constant_data->range_data_3d.returns.Decompress(),
node.constant_data->range_data.returns.Decompress(),
node.pose.cast<float>())),
0.f, &histogram_);
}

18
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -100,14 +100,12 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_);
trajectory_nodes_.Append(
trajectory_id,
mapping::TrajectoryNode{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking),
transform::Rigid3d::Identity()}),
optimized_pose});
trajectory_id, mapping::TrajectoryNode{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time, sensor::Compress(range_data_in_tracking),
transform::Rigid3d::Identity()}),
optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);
@ -200,7 +198,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
// FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
&trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
&trajectory_nodes_.at(node_id).constant_data->range_data.returns,
submap_nodes, initial_relative_pose.rotation(),
&trajectory_connectivity_);
} else {
@ -213,7 +211,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
&trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns,
&trajectory_nodes_.at(node_id).constant_data->range_data.returns,
submap_nodes, initial_relative_pose);
}
}

4
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -68,8 +68,8 @@ class ConstraintBuilder {
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'point_cloud' for 'node_id'. The 'intial_pose' is
// relative to the 'submap'.
// 'submap_id', and the 'compressed_point_cloud' for 'node_id'.
// The 'initial_pose' is relative to the 'submap'.
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.

20
cartographer/sensor/point_cloud.cc
View File

@ -43,25 +43,5 @@ PointCloud Crop(const PointCloud& point_cloud, const float min_z,
return cropped_point_cloud;
}
proto::PointCloud ToProto(const PointCloud& point_cloud) {
proto::PointCloud proto;
for (const auto& point : point_cloud) {
proto.add_x(point.x());
proto.add_y(point.y());
proto.add_z(point.z());
}
return proto;
}
PointCloud ToPointCloud(const proto::PointCloud& proto) {
PointCloud point_cloud;
const int size = std::min({proto.x_size(), proto.y_size(), proto.z_size()});
point_cloud.reserve(size);
for (int i = 0; i != size; ++i) {
point_cloud.emplace_back(proto.x(i), proto.y(i), proto.z(i));
}
return point_cloud;
}
} // namespace sensor
} // namespace cartographer

6
cartographer/sensor/point_cloud.h
View File

@ -42,12 +42,6 @@ PointCloud TransformPointCloud(const PointCloud& point_cloud,
// by 'min_z' and 'max_z'.
PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z);
// Converts 'point_cloud' to a proto::PointCloud.
proto::PointCloud ToProto(const PointCloud& point_cloud);
// Converts 'proto' to a PointCloud.
PointCloud ToPointCloud(const proto::PointCloud& proto);
} // namespace sensor
} // namespace cartographer

17
cartographer/sensor/proto/sensor.proto
View File

@ -20,27 +20,12 @@ option java_outer_classname = "Sensor";
import "cartographer/transform/proto/transform.proto";
// Collection of 3D 'points'.
message PointCloud {
// Points as repeated floats for efficiency. All fields have the same size.
repeated float x = 3 [packed = true];
repeated float y = 4 [packed = true];
repeated float z = 5 [packed = true];
}
// Compressed variant of PointCloud.
// Compressed collection of 3D 'points'.
message CompressedPointCloud {
optional int32 num_points = 1;
repeated int32 point_data = 3 [packed = true];
}
// Proto representation of ::cartographer::sensor::RangeData
message RangeData {
optional transform.proto.Vector3f origin = 1;
optional PointCloud returns = 2;
optional PointCloud misses = 3;
}
// Proto representation of ::cartographer::sensor::ImuData
message ImuData {
optional int64 timestamp = 1;

15
cartographer/sensor/range_data.cc
View File

@ -22,21 +22,6 @@
namespace cartographer {
namespace sensor {
proto::RangeData ToProto(const RangeData& range_data) {
proto::RangeData proto;
*proto.mutable_origin() = transform::ToProto(range_data.origin);
*proto.mutable_returns() = ToProto(range_data.returns);
*proto.mutable_misses() = ToProto(range_data.misses);
return proto;
}
RangeData FromProto(const proto::RangeData& proto) {
return RangeData{
transform::ToEigen(proto.origin()), ToPointCloud(proto.returns()),
ToPointCloud(proto.misses()),
};
}
RangeData TransformRangeData(const RangeData& range_data,
const transform::Rigid3f& transform) {
return RangeData{

6
cartographer/sensor/range_data.h
View File

@ -35,12 +35,6 @@ struct RangeData {
PointCloud misses;
};
// Converts 'range_data' to a proto::RangeData.
proto::RangeData ToProto(const RangeData& range_data);
// Converts 'proto' to a RangeData.
RangeData FromProto(const proto::RangeData& proto);
RangeData TransformRangeData(const RangeData& range_data,
const transform::Rigid3f& transform);

11
cartographer/sensor/range_data_test.cc
View File

@ -65,17 +65,6 @@ TEST_F(RangeDataTest, Compression) {
}
}
TEST_F(RangeDataTest, RangeDataToAndFromProto) {
const auto expected = RangeData{origin_, returns_, misses_};
const auto actual = FromProto(ToProto(expected));
EXPECT_THAT(expected.origin, Near(actual.origin));
EXPECT_THAT(expected.returns,
testing::Pointwise(NearPointwise(), actual.returns));
EXPECT_THAT(expected.misses,
testing::Pointwise(NearPointwise(), actual.misses));
}
TEST_F(RangeDataTest, CompressedRangeDataToAndFromProto) {
const auto expected = CompressedRangeData{
origin_, CompressedPointCloud(returns_), CompressedPointCloud(misses_)};