cartographer_utils.cpp

Go to the documentation of this file.

#include "cartographer_utils.h"
 
#include <cartographer/common/configuration_file_resolver.h>
#include <cartographer/io/proto_stream.h>
#include <cartographer/mapping/map_builder.h>
 
namespace armarx::localization_and_mapping::cartographer_adapter::util
{
 
    std::unique_ptr<::cartographer::common::LuaParameterDictionary>
    resolveLuaParameters(const std::string& luaCode, const std::filesystem::path& configPath)
    {
        assert(fs::is_directory(configPath));
 
        auto fileResolver = std::make_unique<::cartographer::common::ConfigurationFileResolver>(
            std::vector<std::string>{configPath.string()});
        return std::make_unique<::cartographer::common::LuaParameterDictionary>(
            luaCode, std::move(fileResolver));
    }
 
    int64_t
    fromCarto(::cartographer::common::Time time)
    {
        const int64_t utsTimestamp = ::cartographer::common::ToUniversal(time);
 
        int64_t usSinceUnixEpoch =
            (utsTimestamp -
             ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * 10'000'000ll) /
            10;
 
        return usSinceUnixEpoch; // [µs]
    }
 
    ::cartographer::common::Time
    toCarto(const int64_t& time)
    {
        // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000",
        // exactly 719162 days before the Unix epoch.
        return ::cartographer::common::FromUniversal(
            ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * 10'000'000ll +
            time * 10);
    }
 
    std::unique_ptr<::cartographer::mapping::MapBuilderInterface>
    loadMap(const std::filesystem::path& mapPath, const std::filesystem::path& configPath)
    {
        // namespace fs = std::filesystem;
 
        // ARMARX_CHECK(fs::is_regular_file(mapPath));
        // ARMARX_CHECK(fs::is_directory(configPath));
 
        const std::string mapBuilderLuaCode = R"text(
        include "map_builder.lua"
        return MAP_BUILDER)text";
 
        const auto mapBuilderParameters = resolveLuaParameters(mapBuilderLuaCode, configPath);
        const auto mapBuilderOptions =
            ::cartographer::mapping::CreateMapBuilderOptions(mapBuilderParameters.get());
 
        std::unique_ptr<::cartographer::mapping::MapBuilderInterface> mapBuilder =
            ::cartographer::mapping::CreateMapBuilder(mapBuilderOptions);
 
        ::cartographer::io::ProtoStreamReader reader(mapPath.string());
        mapBuilder->LoadState(&reader, true);
        mapBuilder->pose_graph()->RunFinalOptimization();
 
        return mapBuilder;
    }
 
 
} // namespace armarx::localization_and_mapping::cartographer_adapter::util