diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 7ee5ee3..7071ebb 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -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()))); // TODO(whess): Only enable optionally? Resulting pbstream files will be // a lot larger now. writer->WriteProto(proto); diff --git a/cartographer/mapping/proto/serialization.proto b/cartographer/mapping/proto/serialization.proto index d4147d1..5b5b9bf 100644 --- a/cartographer/mapping/proto/serialization.proto +++ b/cartographer/mapping/proto/serialization.proto @@ -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 { diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index 1aac354..a56ff57 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -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 diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index c39baae..d153264 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -113,10 +113,8 @@ void SparsePoseGraph::AddScan( trajectory_id, mapping::TrajectoryNode{ std::make_shared( - 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); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index ad1b8b6..46b663c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -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* 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, diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index 6cece92..023a2fb 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h @@ -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) EXCLUDES(mutex_); diff --git a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc index c35d667..f1fc09e 100644 --- a/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/rotational_scan_matcher.cc @@ -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())), 0.f, &histogram_); } diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 25635c4..a8f4eeb 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -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( - 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( + 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); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 7adbdf5..f038061 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -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. diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index 345a821..bddd85b 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -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 diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index d9d9f66..c2801e2 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -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 diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 354532c..92ea0ba 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -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; diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index db23de0..69c7d58 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -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{ diff --git a/cartographer/sensor/range_data.h b/cartographer/sensor/range_data.h index 91001f7..85e769f 100644 --- a/cartographer/sensor/range_data.h +++ b/cartographer/sensor/range_data.h @@ -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); diff --git a/cartographer/sensor/range_data_test.cc b/cartographer/sensor/range_data_test.cc index 99a3878..24c52e3 100644 --- a/cartographer/sensor/range_data_test.cc +++ b/cartographer/sensor/range_data_test.cc @@ -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_)};