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

Fill submaps slices (#1353) 

This implements a frequently used function (two times in cartographer_ros).
Also, it corrects a wrong repeated proto field access,
so we can fix cartographer_ros/issues/944.
master
gaschler 2018-07-31 11:48:04 +02:00 committed by GitHub
parent c041635fb5
commit c1fbb6bb07
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 41 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
cartographer/io/submap_painter.cc
View File

@ -57,6 +57,16 @@ void CairoPaintSubmapSlices(
}
}
bool Has2DGrid(const mapping::proto::Submap& submap) {
return submap.has_submap_2d() && submap.submap_2d().has_grid();
}
bool Has3DGrids(const mapping::proto::Submap& submap) {
return submap.has_submap_3d() &&
submap.submap_3d().has_low_resolution_hybrid_grid() &&
submap.submap_3d().has_high_resolution_hybrid_grid();
}
} // namespace
PaintSubmapSlicesResult PaintSubmapSlices(
@ -139,6 +149,31 @@ void FillSubmapSlice(
texture_proto.height(), &submap_slice->cairo_data);
}
void DeserializeAndFillSubmapSlices(
ProtoStreamDeserializer* deserializer,
std::map<mapping::SubmapId, SubmapSlice>* submap_slices,
mapping::ValueConversionTables* conversion_tables) {
std::map<mapping::SubmapId, transform::Rigid3d> submap_poses;
for (const auto& trajectory : deserializer->pose_graph().trajectory()) {
for (const auto& submap : trajectory.submap()) {
submap_poses[mapping::SubmapId(trajectory.trajectory_id(),
submap.submap_index())] =
transform::ToRigid3(submap.pose());
}
}
mapping::proto::SerializedData proto;
while (deserializer->ReadNextSerializedData(&proto)) {
if (proto.has_submap() &&
(Has2DGrid(proto.submap()) || Has3DGrids(proto.submap()))) {
const auto& submap = proto.submap();
const mapping::SubmapId id{submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()};
FillSubmapSlice(submap_poses.at(id), submap, &(*submap_slices)[id],
conversion_tables);
}
}
}
SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells,
const int width, const int height) {
SubmapTexture::Pixels pixels;

6
cartographer/io/submap_painter.h
View File

@ -20,6 +20,7 @@
#include "Eigen/Geometry"
#include "cairo/cairo.h"
#include "cartographer/io/image.h"
#include "cartographer/io/proto_stream_deserializer.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/value_conversion_tables.h"
@ -84,6 +85,11 @@ void FillSubmapSlice(
SubmapSlice* const submap_slice,
mapping::ValueConversionTables* conversion_tables);
void DeserializeAndFillSubmapSlices(
ProtoStreamDeserializer* deserializer,
std::map<::cartographer::mapping::SubmapId, SubmapSlice>* submap_slices,
mapping::ValueConversionTables* conversion_tables);
// Unpacks cell data as provided by the backend into 'intensity' and 'alpha'.
SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells,
int width, int height);