Cartographer - disassembling .pbstream into submaps with positions

cartographer-map-ds.cpp

#include <iostream>
#include <map>
#include <string>
#include <vector>

#include "cartographer/io/proto_stream.h"
#include "cartographer/io/proto_stream_deserializer.h"
#include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/value_conversion_tables.h"
#include "cartographer/io/image.h"
#include "cartographer/transform/rigid_transform.h"

#include "gflags/gflags.h"
#include "glog/logging.h"

#include <opencv2/opencv.hpp>

#include <Eigen/Core>

DEFINE_string(pbstream_filename, "map.pbstream", "Filename of a pbstream to draw a map from.");


int main(int argc, char** args)
{
    google::InitGoogleLogging(args[0]);
    google::ParseCommandLineFlags(&argc, &args, true);
    FLAGS_logtostderr = 1;
    CHECK(!FLAGS_pbstream_filename.empty()) << "The option -pbstream_filename is missing. Please define the option to open another file";
    LOG(INFO) << "Loading file " << FLAGS_pbstream_filename;
    cartographer::io::ProtoStreamReader reader(FLAGS_pbstream_filename);
    cartographer::io::ProtoStreamDeserializer deserializer(&reader);
    cartographer::mapping::proto::PoseGraph pose_graph_proto = deserializer.pose_graph();
    const auto& all_builder_options_proto = deserializer.all_trajectory_builder_options();
    std::map<cartographer::mapping::SubmapId, cartographer::transform::Rigid3d> submap_positions;
    for(const cartographer::mapping::proto::Trajectory& trajectory_proto : pose_graph_proto.trajectory())
        for(const cartographer::mapping::proto::Trajectory::Submap& submap_proto : trajectory_proto.submap())
        {
            //LOG(INFO) << "Loading submap position by ID " << submap_proto.submap_index() << " from trajectory " << trajectory_proto.trajectory_id();
            cartographer::mapping::SubmapId current_submap_id{trajectory_proto.trajectory_id(), submap_proto.submap_index()};
            submap_positions.insert(std::pair<cartographer::mapping::SubmapId, cartographer::transform::Rigid3d>(current_submap_id, cartographer::transform::ToRigid3(submap_proto.pose())));
        }
    LOG(INFO) << "Sorting submaps";
    cartographer::mapping::proto::SerializedData proto;
    cartographer::mapping::ValueConversionTables conversion_tables;
    cv::Mat map_texture(5000, 5000, CV_8UC4, cv::Scalar(0));
    long count = 0;
    while(deserializer.ReadNextSerializedData(&proto))
    {
        if(proto.has_submap() &&
                (
                    (proto.submap().has_submap_2d() && proto.submap().submap_2d().has_grid())  ||
                    (proto.submap().has_submap_3d() && proto.submap().submap_3d().has_low_resolution_hybrid_grid() && proto.submap().submap_3d().has_high_resolution_hybrid_grid())
                    )
                )
        {
            cartographer::mapping::SubmapId current_submap_id{proto.submap().submap_id().trajectory_id(), proto.submap().submap_id().submap_index()};
            cartographer::transform::Rigid3d global_pose = submap_positions[current_submap_id];
            const auto& submap = proto.submap();
            cartographer::mapping::proto::SubmapQuery::Response submap_response;
            cartographer::transform::Rigid3d local_pose;
            double resolution;
            if (submap.has_submap_3d())
            {
                cartographer::mapping::Submap3D submap_3d(submap.submap_3d());
                local_pose = submap_3d.local_pose();
                submap_3d.ToResponseProto(global_pose, &submap_response);
            }
            else
            {
                cartographer::mapping::Submap2D submap_2d(submap.submap_2d(), &conversion_tables);
                local_pose = submap_2d.local_pose();
                submap_2d.ToResponseProto(global_pose, &submap_response);
            }
            const auto& texture_proto = submap_response.textures(0);
            //LOG(INFO) << "Submap " << current_submap_id.trajectory_id << ":" << current_submap_id.submap_index << " loaded, unpacking";
            std::string cells;
            cartographer::common::FastGunzipString(texture_proto.cells(), &cells);
            CHECK_EQ(cells.size(), 2 * texture_proto.width() * texture_proto.height());
            long width = texture_proto.width();
            long height = texture_proto.height();
            size_t array_len = cells.size();
            cv::Mat submap_texture(width, height, CV_8UC1);
            cv::Mat submap_intensity(width, height, CV_8UC1);
            cv::Mat submap_image(width, height, CV_8UC4);
            resolution = texture_proto.resolution();
            cartographer::transform::Rigid3d slice_pose = cartographer::transform::ToRigid3(texture_proto.slice_pose());
            for (int i = 0; i < height; ++i)
                for (int j = 0; j < width; ++j)
                {
                    submap_texture.data[(width * height) - ((j * height) + i) - 1] = cells[(((i * width) + (height - j - 1)) * 2)];
                    submap_intensity.data[(width * height) - ((j * height) + i) - 1] = cells[((((i * width) + (height - j - 1)) * 2) + 1)];
                }
            std::vector<cv::Mat> channels;
            for(size_t i = 0; i < 3; i++)
                channels.push_back(submap_texture);
            channels.push_back(submap_intensity);
            cv::merge(channels, submap_image);
            cartographer::transform::Rigid3d vector_pose = slice_pose * global_pose;
            std::cout << "Submap (" << width << "x" << height <<") global pose: [" << global_pose.DebugString() << "], slice pose: [" << slice_pose.DebugString() << "]" << std::endl;
            if(count > 30 && count < 35)
            {
                //cv::imwrite("submap-" + std::to_string(current_submap_id.trajectory_id) + "_" + std::to_string(current_submap_id.submap_index) + ".png", submap_image);
                cv::Rect dest_rect(cv::Point(2500 + static_cast<int>(slice_pose.translation().x() / resolution), 2500 - static_cast<int>(slice_pose.translation().y() / resolution)), cv::Size(height, width));
                cv::add(submap_image, map_texture(dest_rect), map_texture(dest_rect));
            }
            count++;
        }
    }
    cv::imwrite("cartographer-map.png", map_texture);
    return 0;
}