00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/io/submap_painter.h"
00018
00019 #include "cartographer/mapping/2d/submap_2d.h"
00020 #include "cartographer/mapping/3d/submap_3d.h"
00021
00022 namespace cartographer {
00023 namespace io {
00024 namespace {
00025
00026 Eigen::Affine3d ToEigen(const ::cartographer::transform::Rigid3d& rigid3) {
00027 return Eigen::Translation3d(rigid3.translation()) * rigid3.rotation();
00028 }
00029
00030 void CairoPaintSubmapSlices(
00031 const double scale,
00032 const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps,
00033 cairo_t* cr, std::function<void(const SubmapSlice&)> draw_callback) {
00034 cairo_scale(cr, scale, scale);
00035
00036 for (auto& pair : submaps) {
00037 const auto& submap_slice = pair.second;
00038 if (submap_slice.surface == nullptr) {
00039 return;
00040 }
00041 const Eigen::Matrix4d homo =
00042 ToEigen(submap_slice.pose * submap_slice.slice_pose).matrix();
00043
00044 cairo_save(cr);
00045 cairo_matrix_t matrix;
00046 cairo_matrix_init(&matrix, homo(1, 0), homo(0, 0), -homo(1, 1), -homo(0, 1),
00047 homo(0, 3), -homo(1, 3));
00048 cairo_transform(cr, &matrix);
00049
00050 const double submap_resolution = submap_slice.resolution;
00051 cairo_scale(cr, submap_resolution, submap_resolution);
00052
00053
00054
00055 draw_callback(submap_slice);
00056 cairo_restore(cr);
00057 }
00058 }
00059
00060 bool Has2DGrid(const mapping::proto::Submap& submap) {
00061 return submap.has_submap_2d() && submap.submap_2d().has_grid();
00062 }
00063
00064 bool Has3DGrids(const mapping::proto::Submap& submap) {
00065 return submap.has_submap_3d() &&
00066 submap.submap_3d().has_low_resolution_hybrid_grid() &&
00067 submap.submap_3d().has_high_resolution_hybrid_grid();
00068 }
00069
00070 }
00071
00072 PaintSubmapSlicesResult PaintSubmapSlices(
00073 const std::map<::cartographer::mapping::SubmapId, SubmapSlice>& submaps,
00074 const double resolution) {
00075 Eigen::AlignedBox2f bounding_box;
00076 {
00077 auto surface = MakeUniqueCairoSurfacePtr(
00078 cairo_image_surface_create(kCairoFormat, 1, 1));
00079 auto cr = MakeUniqueCairoPtr(cairo_create(surface.get()));
00080 const auto update_bounding_box = [&bounding_box, &cr](double x, double y) {
00081 cairo_user_to_device(cr.get(), &x, &y);
00082 bounding_box.extend(Eigen::Vector2f(x, y));
00083 };
00084
00085 CairoPaintSubmapSlices(
00086 1. / resolution, submaps, cr.get(),
00087 [&update_bounding_box](const SubmapSlice& submap_slice) {
00088 update_bounding_box(0, 0);
00089 update_bounding_box(submap_slice.width, 0);
00090 update_bounding_box(0, submap_slice.height);
00091 update_bounding_box(submap_slice.width, submap_slice.height);
00092 });
00093 }
00094
00095 const int kPaddingPixel = 5;
00096 const Eigen::Array2i size(
00097 std::ceil(bounding_box.sizes().x()) + 2 * kPaddingPixel,
00098 std::ceil(bounding_box.sizes().y()) + 2 * kPaddingPixel);
00099 const Eigen::Array2f origin(-bounding_box.min().x() + kPaddingPixel,
00100 -bounding_box.min().y() + kPaddingPixel);
00101
00102 auto surface = MakeUniqueCairoSurfacePtr(
00103 cairo_image_surface_create(kCairoFormat, size.x(), size.y()));
00104 {
00105 auto cr = MakeUniqueCairoPtr(cairo_create(surface.get()));
00106 cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.);
00107 cairo_paint(cr.get());
00108 cairo_translate(cr.get(), origin.x(), origin.y());
00109 CairoPaintSubmapSlices(1. / resolution, submaps, cr.get(),
00110 [&cr](const SubmapSlice& submap_slice) {
00111 cairo_set_source_surface(
00112 cr.get(), submap_slice.surface.get(), 0., 0.);
00113 cairo_paint(cr.get());
00114 });
00115 cairo_surface_flush(surface.get());
00116 }
00117 return PaintSubmapSlicesResult(std::move(surface), origin);
00118 }
00119
00120 void FillSubmapSlice(
00121 const ::cartographer::transform::Rigid3d& global_submap_pose,
00122 const ::cartographer::mapping::proto::Submap& proto,
00123 SubmapSlice* const submap_slice,
00124 mapping::ValueConversionTables* conversion_tables) {
00125 ::cartographer::mapping::proto::SubmapQuery::Response response;
00126 ::cartographer::transform::Rigid3d local_pose;
00127 if (proto.has_submap_3d()) {
00128 mapping::Submap3D submap(proto.submap_3d());
00129 local_pose = submap.local_pose();
00130 submap.ToResponseProto(global_submap_pose, &response);
00131 } else {
00132 ::cartographer::mapping::Submap2D submap(proto.submap_2d(),
00133 conversion_tables);
00134 local_pose = submap.local_pose();
00135 submap.ToResponseProto(global_submap_pose, &response);
00136 }
00137 submap_slice->pose = global_submap_pose;
00138
00139 auto& texture_proto = response.textures(0);
00140 const SubmapTexture::Pixels pixels = UnpackTextureData(
00141 texture_proto.cells(), texture_proto.width(), texture_proto.height());
00142 submap_slice->width = texture_proto.width();
00143 submap_slice->height = texture_proto.height();
00144 submap_slice->resolution = texture_proto.resolution();
00145 submap_slice->slice_pose =
00146 ::cartographer::transform::ToRigid3(texture_proto.slice_pose());
00147 submap_slice->surface =
00148 DrawTexture(pixels.intensity, pixels.alpha, texture_proto.width(),
00149 texture_proto.height(), &submap_slice->cairo_data);
00150 }
00151
00152 void DeserializeAndFillSubmapSlices(
00153 ProtoStreamDeserializer* deserializer,
00154 std::map<mapping::SubmapId, SubmapSlice>* submap_slices,
00155 mapping::ValueConversionTables* conversion_tables) {
00156 std::map<mapping::SubmapId, transform::Rigid3d> submap_poses;
00157 for (const auto& trajectory : deserializer->pose_graph().trajectory()) {
00158 for (const auto& submap : trajectory.submap()) {
00159 submap_poses[mapping::SubmapId(trajectory.trajectory_id(),
00160 submap.submap_index())] =
00161 transform::ToRigid3(submap.pose());
00162 }
00163 }
00164 mapping::proto::SerializedData proto;
00165 while (deserializer->ReadNextSerializedData(&proto)) {
00166 if (proto.has_submap() &&
00167 (Has2DGrid(proto.submap()) || Has3DGrids(proto.submap()))) {
00168 const auto& submap = proto.submap();
00169 const mapping::SubmapId id{submap.submap_id().trajectory_id(),
00170 submap.submap_id().submap_index()};
00171 FillSubmapSlice(submap_poses.at(id), submap, &(*submap_slices)[id],
00172 conversion_tables);
00173 }
00174 }
00175 }
00176
00177 SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells,
00178 const int width, const int height) {
00179 SubmapTexture::Pixels pixels;
00180 std::string cells;
00181 ::cartographer::common::FastGunzipString(compressed_cells, &cells);
00182 const int num_pixels = width * height;
00183 CHECK_EQ(cells.size(), 2 * num_pixels);
00184 pixels.intensity.reserve(num_pixels);
00185 pixels.alpha.reserve(num_pixels);
00186 for (int i = 0; i < height; ++i) {
00187 for (int j = 0; j < width; ++j) {
00188 pixels.intensity.push_back(cells[(i * width + j) * 2]);
00189 pixels.alpha.push_back(cells[(i * width + j) * 2 + 1]);
00190 }
00191 }
00192 return pixels;
00193 }
00194
00195 UniqueCairoSurfacePtr DrawTexture(const std::vector<char>& intensity,
00196 const std::vector<char>& alpha,
00197 const int width, const int height,
00198 std::vector<uint32_t>* const cairo_data) {
00199 CHECK(cairo_data->empty());
00200
00201
00202
00203 const int expected_stride = 4 * width;
00204 CHECK_EQ(expected_stride, cairo_format_stride_for_width(kCairoFormat, width));
00205 for (size_t i = 0; i < intensity.size(); ++i) {
00206
00207
00208 const uint8_t intensity_value = intensity.at(i);
00209 const uint8_t alpha_value = alpha.at(i);
00210 const uint8_t observed =
00211 (intensity_value == 0 && alpha_value == 0) ? 0 : 255;
00212 cairo_data->push_back((alpha_value << 24) | (intensity_value << 16) |
00213 (observed << 8) | 0);
00214 }
00215
00216 auto surface = MakeUniqueCairoSurfacePtr(cairo_image_surface_create_for_data(
00217 reinterpret_cast<unsigned char*>(cairo_data->data()), kCairoFormat, width,
00218 height, expected_stride));
00219 CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS)
00220 << cairo_status_to_string(cairo_surface_status(surface.get()));
00221 return surface;
00222 }
00223
00224 }
00225 }