CartographerAdapter.cpp

Go to the documentation of this file.

#include "CartographerAdapter.h"
 
// STL
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <filesystem>
#include <functional>
#include <iterator>
#include <memory>
#include <mutex>
#include <optional>
#include <stdexcept>
#include <string>
 
// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
 
// Ice
#include <IceUtil/Time.h>
 
// PCL
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
// OpenCV
#include <opencv2/core/mat.hpp>
#include <opencv2/imgcodecs.hpp>
 
// Simox
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/json/json.hpp>
#include <SimoxUtility/math.h>
 
// ArmarX
#include "ArmarXCore/util/CPPUtility/trace.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/algorithm.h>
 
// RobotComponents
#include <armarx/localization_and_mapping/cartographer_adapter/interfaces.h>
 
// cartographer (this has to come last!)
#include <cartographer/common/configuration_file_resolver.h>
#include <cartographer/common/time.h>
#include <cartographer/io/file_writer.h>
#include <cartographer/io/points_processor_pipeline_builder.h>
#include <cartographer/io/proto_stream.h>
#include <cartographer/mapping/2d/grid_2d.h>
#include <cartographer/mapping/2d/probability_grid.h>
#include <cartographer/mapping/2d/submap_2d.h>
#include <cartographer/mapping/internal/2d/tsdf_2d.h>
#include <cartographer/mapping/map_builder.h>
#include <cartographer/mapping/map_builder_interface.h>
#include <cartographer/mapping/pose_graph_interface.h>
#include <cartographer/mapping/submaps.h>
#include <cartographer/mapping/trajectory_builder_interface.h>
#include <cartographer/mapping/trajectory_node.h>
#include <cartographer/sensor/point_cloud.h>
#include <cartographer/sensor/rangefinder_point.h>
#include <cartographer/transform/rigid_transform.h>
#include <cartographer/transform/transform.h>
 
// this library
#include "ApproximateTimeQueue.h"
#include "ArVizDrawer.h"
#include "grid_conversion.h"
#include "types.h"
#include "util/cartographer_utils.h"
#include "util/eigen_conversions.h"
#include "util/laser_scanner_conversion.h"
#include "util/pcl_conversions.h"
 
namespace carto = ::cartographer;
 
constexpr int SCALE_M_TO_MM = 1000;
 
namespace fs = ::std::filesystem;
 
std::string
timestamp()
{
    // http://www.cplusplus.com/reference/ctime/strftime/
 
    time_t rawtime{};
    struct tm* timeinfo{};
    std::array<char, 4 + 1 + 2 + 1 + 2 + 1 + 2 + 1 + 2 + 1> buffer;
 
    time(&rawtime);
    timeinfo = localtime(&rawtime);
 
    strftime(buffer.data(), buffer.size(), "%Y-%m-%d-%H-%M", timeinfo);
 
    std::string timestamp(buffer.data());
    return timestamp;
}
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
    CartographerAdapter::~CartographerAdapter()
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "Finishing cartographer";
        mapBuilder->FinishTrajectory(trajectoryId);
 
        ARMARX_INFO << "Releasing cartographer";
        mapBuilder.reset();
 
        ARMARX_INFO << "Done";
    }
 
    CartographerAdapter::CartographerAdapter(
        const std::filesystem::path& mapPath,
        const std::filesystem::path& configPath,
        SlamDataCallable& slamDataCallable,
        const Config& config,
        const std::optional<std::filesystem::path>& mapToLoad,
        const std::optional<Eigen::Isometry3f>& map_T_robot_prior,
        const DebugObserverInterfacePrx& debugObserver) :
        config(config), mapPath(mapPath), slamDataCallable(slamDataCallable)
    {
        ARMARX_TRACE;
        const std::string mapBuilderLuaCode = R"text(
          include "map_builder.lua"
          return MAP_BUILDER)text";
 
        const std::string trajectoryBuilderLuaCode = R"text(
          include "trajectory_builder.lua"
          return TRAJECTORY_BUILDER)text";
 
        const auto mapBuilderParameters = util::resolveLuaParameters(mapBuilderLuaCode, configPath);
        auto mapBuilderOptions =
            carto::mapping::CreateMapBuilderOptions(mapBuilderParameters.get());
 
 
        if (config.global_localization_min_score > 0)
        {
            ARMARX_INFO << "Setting `global_localization_min_score` to "
                        << config.global_localization_min_score;
            mapBuilderOptions.mutable_pose_graph_options()
                ->mutable_constraint_builder_options()
                ->set_global_localization_min_score(config.global_localization_min_score);
        }
 
        if (config.min_score > 0)
        {
            ARMARX_INFO << "Setting `min_score` to " << config.min_score;
            mapBuilderOptions.mutable_pose_graph_options()
                ->mutable_constraint_builder_options()
                ->set_min_score(config.min_score);
        }
 
        const auto trajectoryBuilderParameters =
            util::resolveLuaParameters(trajectoryBuilderLuaCode, configPath);
        trajectoryBuilderOptions =
            carto::mapping::CreateTrajectoryBuilderOptions(trajectoryBuilderParameters.get());
 
        // create map builder
        mapBuilder = carto::mapping::CreateMapBuilder(mapBuilderOptions);
 
        if (config.useLaserScanner)
        {
            sensorSet.insert(sensorIdLaser);
        }
 
        if (config.useOdometry)
        {
            sensorSet.insert(sensorIdOdom);
        }
 
        if (config.useImu)
        {
            sensorSet.insert(sensorIdIMU);
        }
 
        std::vector<std::string> sensorSetNames;
        std::transform(sensorSet.begin(),
                       sensorSet.end(),
                       std::back_inserter(sensorSetNames),
                       [](const auto& sensorId) { return sensorId.id; });
 
        ARMARX_IMPORTANT << "Sensor set '" << sensorSetNames << "' will be used";
 
        if (config.useLaserScanner)
        {
            ARMARX_IMPORTANT << "This includes the following laser scanners: "
                             << std::vector<std::string>(config.laserScanners.begin(),
                                                         config.laserScanners.end());
 
            std::for_each(config.laserScanners.begin(),
                          config.laserScanners.end(),
                          [&](const auto& laserScannerName)
                          {
                              const SensorId sensorId{.type = SensorId::SensorType::RANGE,
                                                      .id = laserScannerName};
                              laserSensorMap.insert(
                                  {laserScannerName,
                                   LaserScannerSensor(sensorId, Eigen::Isometry3f::Identity())});
                          });
        }
 
 
        if (mapToLoad)
        {
            ARMARX_TRACE;
            ARMARX_DEBUG << "Loading map";
            carto::io::ProtoStreamReader reader(mapToLoad->string());
            mapBuilder->LoadState(&reader, true /* load_frozen_state */);
            mapBuilder->pose_graph()->RunFinalOptimization();
 
 
            if (map_T_robot_prior)
            {
                ARMARX_INFO << "Using pose prior \n" << map_T_robot_prior->matrix();
 
                ::cartographer::mapping::proto::InitialTrajectoryPose* initial_trajectory_pose =
                    new ::cartographer::mapping::proto::InitialTrajectoryPose;
 
                initial_trajectory_pose->set_allocated_relative_pose(
                    new ::cartographer::transform::proto::Rigid3d(
                        ::cartographer::transform::ToProto(fromEigen(*map_T_robot_prior))));
 
                trajectoryBuilderOptions.set_allocated_initial_trajectory_pose(
                    std::move(initial_trajectory_pose)); // owner -> trajectoryBuilderOptions
            }
        }
 
 
        trajectoryId = mapBuilder->AddTrajectoryBuilder(
            sensorSet,
            trajectoryBuilderOptions,
            [this](auto&&... args)
            { return getLocalSlamResultCallback(std::forward<decltype(args)>(args)...); });
 
        // TODO(fabian.reister): variable sensor set
        const auto dt = static_cast<int64_t>(1'000'000 / config.frequency);
        const float dtHistoryLength = 1'000'000;
 
        ARMARX_IMPORTANT << "Frequency: " << config.frequency << ", dt: " << dt;
 
        approxTimeQueue =
            std::make_unique<ApproximateTimeQueue>(dt,
                                                   dtHistoryLength,
                                                   config.laserScanners,
                                                   *this,
                                                   config.useOdometryBasedLaserCorrection);
 
        if (debugObserver)
        {
            ARMARX_TRACE;
            ARMARX_IMPORTANT << "Debug observer is set";
            cartographerInputDataReporterLaserScanner.emplace(
                debugObserver, "CartographerMappingAndLocalization_input_laser");
            cartographerInputDataReporterOdometry.emplace(
                debugObserver, "CartographerMappingAndLocalization_input_odom");
            if (config.enableDebugTiming)
            {
                debugObserverHelper.emplace(
                    "CartographerMappingAndLocalization_CartographerAdapter", debugObserver, true);
            }
        }
    }
 
    void
    CartographerAdapter::onTimedDataAvailable(const TimedData& data)
    {
        ARMARX_TRACE;
        pushInOdomData(data.odomPose);
        pushInLaserData(data.laserData);
        if (debugObserverHelper)
        {
            ARMARX_TRACE;
            std::unique_lock g(debugObserverHelperMtx);
            debugObserverHelper->sendDebugObserverBatch();
        }
    }
 
    void
    CartographerAdapter::pushInOdomData(
        const armarx::localization_and_mapping::cartographer_adapter::PoseStamped& odomPose)
    {
        ARMARX_TRACE;
        const DebugObserverScopedTimer timer(this, "pushInOdomData | Entire Method");
        if (lastOdomTimestamp.has_value())
        {
            ARMARX_TRACE;
            if (odomPose.timestamp <= lastOdomTimestamp.value())
            {
                ARMARX_DEBUG
                    << "Requested to add old data into odom queue. Not willing to do this. It is "
                    << (odomPose.timestamp - lastOdomTimestamp.value()) << "ms in the past.";
                return;
            }
        }
 
        lastOdomTimestamp = odomPose.timestamp;
 
        const auto& timestamp = odomPose.timestamp;
        ARMARX_DEBUG << "New odom pose received " << timestamp;
 
        auto isOdomSensor = [](const SensorId& sensorId)
        { return sensorId.type == SensorId::SensorType::ODOMETRY; };
 
        if (not std::any_of(sensorSet.begin(), sensorSet.end(), isOdomSensor))
        {
            ARMARX_TRACE;
            ARMARX_WARNING << "No odometry sensor is registered. Will not use odometry data.";
            ARMARX_DEBUG << deactivateSpam(2.) << "Odometry will not be used";
            return;
        }
 
        ARMARX_DEBUG << "Processing odom pose";
 
        const Eigen::Quaternionf q(odomPose.pose.linear());
        const Eigen::Vector3f p = odomPose.pose.translation();
 
        carto::sensor::OdometryData odomData;
        odomData.pose =
            carto::transform::Rigid3d::FromArrays(std::array<double, 4>{q.w(), q.x(), q.y(), q.z()},
                                                  std::array<double, 3>{p.x(), p.y(), p.z()});
        odomData.time = util::toCarto(timestamp);
 
        ARMARX_VERBOSE << "Will now insert odom data";
 
        {
            ARMARX_TRACE;
            const DebugObserverScopedTimer localTimer(this, "pushInOdomData | Inserting Odom Data");
            std::lock_guard guard{cartoInsertMtx};
 
            mapBuilder->GetTrajectoryBuilder(trajectoryId)
                ->AddSensorData(sensorIdOdom.id, odomData);
        }
 
        if (cartographerInputDataReporterOdometry.has_value())
        {
            ARMARX_TRACE;
            cartographerInputDataReporterOdometry->add(timestamp);
        }
 
        ARMARX_DEBUG << "Inserted odom data";
    }
 
    void
    CartographerAdapter::pushInLaserData(const LaserMessages& laserMessages)
    {
        ARMARX_TRACE;
 
        if (not config.useLaserScanner)
        {
            ARMARX_INFO << deactivateSpam(100) << "Not considering laser scanner data.";
            return;
        }
 
        const DebugObserverScopedTimer timer(this, "pushInLaserData | Entire Method");
        carto::sensor::TimedPointCloudData timedPointCloudData;
        timedPointCloudData.origin = Eigen::Vector3f::Zero();
 
        {
            ARMARX_TRACE;
            const DebugObserverScopedTimer laserMessageTimer(
                this, "pushInLaserData | Iterate over laser messages");
            for (const auto& message : laserMessages)
            {
                const auto& data = message.data;
 
                ARMARX_VERBOSE << "pushInLaserData" << data.scan.size() << " for "
                               << message.data.frame;
 
                // validate timestamp
                {
                    // First iteration: lastTimestamp will be created and initialized to 0
                    auto& lastTimestamp = lastSensorDataTimestamp[data.frame];
 
                    if (data.timestamp <= lastTimestamp)
                    {
                        ARMARX_DEBUG << "Requested to add old data with timestamp "
                                     << data.timestamp << " into laser queue for sensor "
                                     << data.frame << ". Not willing to do this. It is "
                                     << (data.timestamp - lastTimestamp) << "ms in the past.";
                        return;
                    }
 
                    lastTimestamp = data.timestamp;
                }
 
                // assumption: all points from single sensor
                Eigen::Isometry3f robot_T_sensor;
                try
                {
                    const LaserScannerSensor sensor = lookupSensor(data.frame);
                    ARMARX_DEBUG << "Sensor " << data.frame << " position is "
                                 << sensor.pose.translation();
 
                    robot_T_sensor = sensor.pose;
                }
                catch (const armarx::LocalException&)
                {
                    ARMARX_INFO << "Sensor `" << data.frame << "` not found in sensor set.";
                    return;
                }
 
                // [m]
 
                {
                    const DebugObserverScopedTimer localTimer(
                        this, "pushInLaserData | Reserve TimedPointCloudData");
                    timedPointCloudData.time =
                        util::toCarto(data.timestamp); // universal is in 100ns
                    timedPointCloudData.ranges.reserve(timedPointCloudData.ranges.size() +
                                                       data.scan.size());
                }
                ARMARX_DEBUG << "Laser range data: " << data.scan.size() << " points";
 
                {
                    const DebugObserverScopedTimer localTimer(
                        this, "pushInLaserData | Transform to TimedPointCloudData");
                    std::transform(data.scan.begin(),
                                   data.scan.end(),
                                   std::back_inserter(timedPointCloudData.ranges),
                                   [&](const auto& scanStep) -> carto::sensor::TimedRangefinderPoint
                                   {
                                       auto rangePoint =
                                           toCartesian<Eigen::Vector3f>(scanStep); // [mm]
                                       rangePoint /= 1000; // [mm] -> [m]
 
                                       carto::sensor::TimedRangefinderPoint point;
                                       // point.position = robot_time_correction_T_sensor* rangePoint;
                                       point.position = robot_T_sensor * rangePoint;
                                       point.time = .0; // relative measurement time (not known)
                                       return point;
                                   });
                }
            }
        }
 
        auto cloud = toPCL(timedPointCloudData.ranges);
        {
            const DebugObserverScopedTimer localTimer(this, "pushInLaserData | onLaserSensorData");
            ARMARX_VERBOSE << VAROUT(cloud.size());
            slamDataCallable.onLaserSensorData({util::fromCarto(timedPointCloudData.time), cloud});
        }
        {
            ARMARX_TRACE;
            const DebugObserverScopedTimer localTimer(this,
                                                      "pushInLaserData | Inserting Laser Data");
            std::lock_guard guard{cartoInsertMtx};
 
            auto* trajBuilder = mapBuilder->GetTrajectoryBuilder(trajectoryId);
            if (trajBuilder == nullptr)
            {
                ARMARX_WARNING << "No trajectory builder.";
                return;
            }
 
            const std::int64_t laserScannerInsertCartoTimestamp =
                timedPointCloudData.time.time_since_epoch().count();
 
            ARMARX_DEBUG << "Inserting " << sensorIdLaser.id << " with timestamp "
                         << laserScannerInsertCartoTimestamp;
 
            if (lastLaserScannerInsertCartoTimestamp.has_value())
            {
                if (laserScannerInsertCartoTimestamp <= lastLaserScannerInsertCartoTimestamp)
                {
                    ARMARX_INFO << "Received laser scanner data out of order. Will not handle it.";
                    return;
                }
            }
 
            lastLaserScannerInsertCartoTimestamp = laserScannerInsertCartoTimestamp;
 
            {
                const DebugObserverScopedTimer addSensorDataTimer(
                    this, "pushInLaserData | Add Sensor Data to TrajectoryBuilder");
                ARMARX_VERBOSE << VAROUT(timedPointCloudData.ranges.size());
                trajBuilder->AddSensorData(sensorIdLaser.id, timedPointCloudData);
            }
 
            if (cartographerInputDataReporterLaserScanner)
            {
                cartographerInputDataReporterLaserScanner->add(
                    Duration::MicroSeconds(util::fromCarto(timedPointCloudData.time)));
            }
        }
 
 
        ARMARX_DEBUG << "Inserted laser data";
    }
 
    void
    CartographerAdapter::processOdometryPose(OdomData odom)
    {
        ARMARX_TRACE;
        const DebugObserverScopedTimer timer(this, "processOdometryPose | Entire Method");
        armarx::localization_and_mapping::cartographer_adapter::PoseStamped odomPose;
 
        odomPose.timestamp = odom.timestamp;
        odomPose.pose.setIdentity();
        odomPose.pose.translation() = odom.position;
        odomPose.pose.linear() = odom.orientation.toRotationMatrix();
 
        approxTimeQueue->insertOdomData(std::move(odomPose));
    }
 
    void
    CartographerAdapter::processSensorValues(LaserScannerMessage data)
    {
        ARMARX_TRACE;
        const DebugObserverScopedTimer timer(this, "processSensorValues | Entire Method");
        ARMARX_DEBUG << "processing sensor values (intro)";
 
 
        // auto isSensorRegistered = [&](const std::string& sensorId)
        // {
        //     return std::any_of(sensorSet.begin(),
        //                        sensorSet.end(),
        //                        [&sensorId](const SensorId& id) -> bool
        //                        { return id.id == sensorId; });
        // };
 
        // if (not isSensorRegistered(data.frame))
        // {
        //     ARMARX_VERBOSE << deactivateSpam(1.0) << "Sensor " << data.frame << " not registered. Registered sensors: " << sensorSet;
        //     return;
        // }
 
        ARMARX_CHECK_NOT_NULL(approxTimeQueue);
        approxTimeQueue->insertLaserData(std::move(data));
    }
 
    void
    CartographerAdapter::getLocalSlamResultCallback(
        const int trajectoryId,
        const ::cartographer::common::Time time,
        const ::cartographer::transform::Rigid3d localPose,
        ::cartographer::sensor::RangeData rangeDataInLocal,
        const std::unique_ptr<
            const ::cartographer::mapping::TrajectoryBuilderInterface::InsertionResult>
            insertion_result)
    {
        ARMARX_TRACE;
        const DebugObserverScopedTimer timer(this, "getLocalSlamResultCallback | Entire Method");
        // std::lock_guard g{cartoInsertMtx};
 
        ARMARX_DEBUG << "Callback (getLocalSlamResultCallback)";
 
 
        //        std::lock_guard g{cartoInsertMtx};
 
        // const auto constraints = mapBuilder->pose_graph()->constraints();
        // ARMARX_DEBUG << "number of contraints: " << constraints.size();
 
        // const size_t nConstraintsInter = std::count_if(
        //                                      constraints.begin(),
        //                                      constraints.end(),
        //                                      [](const ::cartographer::mapping::PoseGraphInterface::Constraint & constraint)
        // {
        //     return constraint.tag ==
        //            ::cartographer::mapping::PoseGraphInterface::Constraint::INTER_SUBMAP;
        // });
 
        // ARMARX_DEBUG << "Inter submap constraints: " << nConstraintsInter;
 
        LocalSlamData localSlamData;
 
        {
            ARMARX_TRACE;
            const DebugObserverScopedTimer localTimer(
                this, "getLocalSlamResultCallback | Prepare Local Slam Data");
            const auto timestampInMicroSeconds = util::fromCarto(time);
 
            ARMARX_DEBUG << "local slam data timestamp: "
                         << IceUtil::Time::microSeconds(timestampInMicroSeconds);
 
            localSlamData.local_points = toPCL(rangeDataInLocal.returns.points());
            localSlamData.misses = toPCL(rangeDataInLocal.misses.points());
            localSlamData.local_pose = toEigen(localPose);
            localSlamData.timestamp = timestampInMicroSeconds;
            localSlamData.trajectory_pose =
                toEigen(mapBuilder->pose_graph()->GetLocalToGlobalTransform(trajectoryId));
 
            ARMARX_DEBUG << "Callbacks will be triggered (getLocalSlamResultCallback)";
        }
        {
            const DebugObserverScopedTimer localTimer(
                this, "getLocalSlamResultCallback | On Local Slam Data");
            slamDataCallable.onLocalSlamData(localSlamData);
        }
        // triggerCallbacks();
 
        // report status of localization quality
        {
            const auto nConstraints = numConstraints();
 
            if (debugObserverHelper.has_value())
            {
                const std::unique_lock lock(debugObserverHelperMtx);
                debugObserverHelper->setDebugObserverDatafield("Number of constraints",
                                                               nConstraints);
            }
        }
 
        ARMARX_DEBUG << "Callback done (getLocalSlamResultCallback)";
    }
 
    // void CartographerAdapter::triggerCallbacks() const
    // {
    //     // slamDataCallable.onGraphOptimized(optimizedGraphData());
    //     // slamDataCallable.onGridMap(submapData(trajectoryId));
    // }
 
    bool
    CartographerAdapter::hasReceivedSensorData() const noexcept
    {
        ARMARX_TRACE;
        const DebugObserverScopedTimer timer(this, "hasReceivedSensorData | Entire Method");
        std::lock_guard g{cartoInsertMtx};
 
        const auto trajectoryNodes = mapBuilder->pose_graph()->GetTrajectoryNodes();
 
        auto matchesTrajectoryId = [&](const auto& node) -> bool
        { return node.id.trajectory_id == trajectoryId; };
 
        // checks if nodes wrt. trajectory_id exist
        return std::any_of(trajectoryNodes.begin(), trajectoryNodes.end(), matchesTrajectoryId);
    }
 
    bool
    storeDefaultRegistrationFile(const fs::path& jsonFile)
    {
        ARMARX_TRACE;
        std::ofstream ofs(jsonFile, std::ios::out);
 
        ARMARX_CHECK(ofs.is_open());
        ARMARX_CHECK(not ofs.fail());
 
        nlohmann::json data;
        data["x"] = 0.F;
        data["y"] = 0.F;
        data["yaw"] = 0.F;
 
        ARMARX_INFO << "Creating dummy registration file `" << jsonFile << "`";
        ofs << data;
 
        return true;
    }
 
    bool
    CartographerAdapter::createMap(const armarx::PackagePath& mapOutputPath)
    {
        ARMARX_TRACE;
        const std::string filenameNoExtension = timestamp();
 
        const fs::path mapFilename =
            std::filesystem::path(mapOutputPath.toSystemPath()) / (filenameNoExtension + ".carto");
        ARMARX_DEBUG << "The concrete map file is " << mapFilename.string();
        assert(!fs::exists(map_filename));
 
        // Create a new trajectory such that new data will not interfer with the map
        // that we are optimizing. Therefore, keep track of the trajectory that we
        // optimize.
        const int trajectoryIdToBeOptimized = trajectoryId;
 
        std::lock_guard g{cartoInsertMtx};
 
        trajectoryId = mapBuilder->AddTrajectoryBuilder(
            sensorSet,
            trajectoryBuilderOptions,
            [&](auto... args)
            { return getLocalSlamResultCallback(std::forward<decltype(args)>(args)...); });
 
        ARMARX_IMPORTANT << "Optimizing map ...";
        mapBuilder->FinishTrajectory(trajectoryIdToBeOptimized);
        mapBuilder->pose_graph()->RunFinalOptimization();
        ARMARX_IMPORTANT << "... done.";
 
        carto::io::ProtoStreamWriter writer(mapFilename);
        mapBuilder->SerializeState(true, &writer);
        writer.Close();
 
        ARMARX_IMPORTANT << "Saved map to '" << mapFilename << "'";
 
        const fs::path registrationFilename = mapPath / (filenameNoExtension + ".json");
        ARMARX_INFO << "The default registration data will be stored in " << registrationFilename;
        storeDefaultRegistrationFile(registrationFilename);
 
        // slamDataCallable.onGraphOptimized(optimizedGraphData());
        // slamDataCallable.onGridMap(submapData(trajectoryIdToBeOptimized));
 
        return true;
    }
 
    const CartographerAdapter::LaserScannerSensor&
    CartographerAdapter::lookupSensor(const std::string& device) const
    {
        ARMARX_TRACE;
        const DebugObserverScopedTimer timer(this, "lookupSensor");
        auto laserSensorIt = laserSensorMap.find(device);
        if (laserSensorIt == laserSensorMap.end())
        {
            throw armarx::LocalException("No sensor with id '" + device + "' found.");
        }
        return laserSensorIt->second;
    }
 
    void
    CartographerAdapter::setSensorPose(const std::string& sensorId, const Eigen::Isometry3f& pose)
    {
        ARMARX_TRACE;
        auto laserSensorIt = laserSensorMap.find(sensorId);
        if (laserSensorIt == laserSensorMap.end())
        {
            throw armarx::LocalException("No sensor with id '" + sensorId + "' found.");
        }
        laserSensorIt->second.pose = pose;
    }
 
    std::vector<std::string>
    CartographerAdapter::laserSensorIds() const
    {
        ARMARX_TRACE;
        return armarx::getMapKeys(laserSensorMap);
    }
 
    CartographerAdapter::DebugObserverScopedTimer::~DebugObserverScopedTimer()
    {
        ARMARX_TRACE;
        if (parent->debugObserverHelper)
        {
            const armarx::Duration duration = armarx::DateTime::Now() - start;
            const std::unique_lock lock(parent->debugObserverHelperMtx);
            parent->debugObserverHelper->setDebugObserverDatafield(name + " [ms]",
                                                                   duration.toMicroSeconds());
        }
    }
 
    CartographerAdapter::DebugObserverScopedTimer::DebugObserverScopedTimer(
        const CartographerAdapter* parent,
        const std::string& name) :
        parent(parent), name(name), start(armarx::DateTime::Now())
    {
    }
 
    unsigned int
    CartographerAdapter::numConstraints() const
    {
        const auto constraints = mapBuilder->pose_graph()->constraints();
        return std::count_if(
            constraints.begin(),
            constraints.end(),
            [trajectoryId = this->trajectoryId](
                const ::cartographer::mapping::PoseGraphInterface::Constraint& constraint)
            { return constraint.node_id.trajectory_id == trajectoryId; });
    }
 
    ::cartographer::mapping::MapBuilderInterface&
    CartographerAdapter::getMapBuilder()
    {
        return *mapBuilder;
    }
} // namespace armarx::localization_and_mapping::cartographer_adapter