CartographerMapRegistration.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotComponents::ArmarXObjects::CartographerMapRegistration
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CartographerMapRegistration.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <filesystem>
#include <iterator>
#include <memory>
#include <numeric>
#include <utility>
 
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
 
#include <SimoxUtility/json.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/MathTools.h>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/system/ArmarXDataPath.h"
 
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
#include <RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h>
#include <RobotAPI/libraries/armem_vision/types.h>
 
#include "RobotComponents/components/CartographerMappingAndLocalization/CartographerMappingAndLocalization.h"
#include "RobotComponents/libraries/cartographer/map_registration/LaserScanAggregator.h"
#include "RobotComponents/libraries/cartographer/map_registration/MapRegistration.h"
#include "RobotComponents/libraries/cartographer/map_registration/models.h"
#include "RobotComponents/libraries/cartographer/util/cartographer_utils.h"
#include "RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h"
 
namespace fs = std::filesystem;
 
namespace armarx::cartographer
{
 
    CartographerMapRegistration::~CartographerMapRegistration() = default;
 
    armarx::PropertyDefinitionsPtr
    CartographerMapRegistration::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        // Publish to a topic (passing the TopicListenerPrx).
        // def->topic(myTopicListener);
 
        // Subscribe to a topic (passing the topic name).
        // def->topic<PlatformUnitListener>("MyTopic");
 
        // Use (and depend on) another component (passing the ComponentInterfacePrx).
        def->component(priorKnowledge);
        def->component(workingMemory);
        def->component(robotStateComponent);
 
        def->component(remoteGuiInterface,
                       "RemoteGuiProvider",
                       "remote_gui_provider",
                       "Name of the remote gui provider");
 
        // Add a required property.
        def->required(properties.mapToLoad, "p.carto.mapToLoad", "");
 
        // Add an optional property.
        def->optional(properties.cartographerConfigPath, "p.carto.configPath", "");
        def->optional(properties.mapPath, "p.carto.mapPath", "");
 
        def->optional(properties.minLaserRange,
                      "p.algo.minLaserRange",
                      "All points closer to the robot than 'minLaserRange' will be removed from "
                      "the point cloud.");
 
        mappingDataReader->registerPropertyDefinitions(def);
 
        return def;
    }
 
    CartographerMapRegistration::CartographerMapRegistration() :
        mappingDataReader(std::make_unique<armem::vision::laser_scans::client::Reader>(*this)),
        arvizDrawer(arviz)
    {
    }
 
    void
    CartographerMapRegistration::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requies the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
    }
 
    void
    CartographerMapRegistration::onConnectComponent()
    {
        // establish connection to memory
        mappingDataReader->connect();
 
        // Do things after connecting to topics and components.
 
        /* (Requies the armarx::DebugObserverComponentPluginUser.)
        // Use the debug observer to log data over time.
        // The data can be viewed in the ObserverView and the LivePlotter.
        // (Before starting any threads, we don't need to lock mutexes.)
        {
            setDebugObserverDatafield("numBoxes", properties.numBoxes);
            setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
            sendDebugObserverBatch();
        }
        */
 
        /* (Requires the armarx::ArVizComponentPluginUser.)
        // Draw boxes in ArViz.
        // (Before starting any threads, we don't need to lock mutexes.)
        drawBoxes(properties, arviz);
        */
 
        /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
        // Setup the remote GUI.
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
        */
 
        remoteGui = std::make_unique<armarx::cartographer_map_registration::RemoteGui>(
            remoteGuiInterface, *this);
 
        arvizDrawer.clear();
    }
 
    void
    CartographerMapRegistration::onDisconnectComponent()
    {
    }
 
    void
    CartographerMapRegistration::onExitComponent()
    {
    }
 
    std::string
    CartographerMapRegistration::getDefaultName() const
    {
        return "CartographerMapRegistration";
    }
 
    std::vector<TimeRange>
    splitTimeRange(const TimeRange& timeRange, const int64_t& maxDuration)
    {
        const int64_t duration = timeRange.duration().toMicroSeconds();
        const size_t nSplits = std::ceil(static_cast<float>(duration) / maxDuration);
 
        std::vector<TimeRange> timeRanges;
 
        for (size_t i = 0; i < nSplits; i++)
        {
            TimeRange tr{.min = IceUtil::Time::microSeconds(static_cast<int64_t>(
                             timeRange.min.toMicroSeconds() + maxDuration * i)),
                         .max = IceUtil::Time::microSeconds(static_cast<int64_t>(
                             timeRange.min.toMicroSeconds() + maxDuration * (i + 1)))};
 
            tr.max = std::min(tr.max, timeRange.max);
 
            timeRanges.push_back(tr);
        }
 
        return timeRanges;
    }
 
    void
    CartographerMapRegistration::loadGraph()
    {
        ARMARX_IMPORTANT << "Loading SLAM graph";
 
        arvizDrawer.drawFrames(world_T_map);
 
        const std::string configSubfolder = "mapping";
        const fs::path mapPath = ArmarXDataPath::resolvePath(properties.mapPath);
 
        const auto configPath =
            fs::path(ArmarXDataPath::resolvePath(properties.cartographerConfigPath)) /
            configSubfolder;
 
        ARMARX_IMPORTANT << "Using config files from " << configPath;
        ARMARX_CHECK(fs::is_directory(configPath));
 
        // load the map in localization mode
        const fs::path mapToLoad = mapPath / properties.mapToLoad;
 
        // (1) get the map name / load the map
        ARMARX_IMPORTANT << "Loading map from " << mapToLoad.string();
        ARMARX_CHECK(fs::is_regular_file(mapToLoad));
 
        mapRegistration = std::make_unique<MapRegistration>(loadMap(mapToLoad, configPath));
 
        const auto robotPoses = mapRegistration->robotPoses();
        arvizDrawer.drawRobotPoses(robotPoses, world_T_map);
    }
 
    void
    CartographerMapRegistration::loadDataFromMemory()
    {
 
        // (2) get all data from the memory
        ARMARX_IMPORTANT << "Getting sensor data from memory";
 
        const TimeRange fullTimeRange = mapRegistration->mapTimeRange();
        const auto timeRanges = splitTimeRange(fullTimeRange, 10'000'000'000);
 
        auto robot = RemoteRobot::createLocalClone(robotStateComponent);
        RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
        const std::string agentName = robot->getName();
 
        for (const auto& timeRange : timeRanges)
        {
            const armem::vision::laser_scans::client::Reader::Query query{
                .agent = agentName,
                .timeRange = {timeRange.min, timeRange.max},
                .sensorList = {} // all
            };
 
            const armem::vision::laser_scans::client::Reader::Result result =
                mappingDataReader->query(query);
 
            ARMARX_IMPORTANT << "Received data from memory";
            ARMARX_IMPORTANT << "Number of point clouds: " << result.laserScans.size();
 
            const auto& laserScans = result.laserScans;
 
            if (not aggregator)
            {
                const auto& sensorFrames = result.sensors;
 
                const auto sensorPose = [=](const std::string& sensorFrame) -> Eigen::Affine3f
                { return Eigen::Affine3f(robot->getRobotNode(sensorFrame)->getPoseInRootFrame()); };
 
                LaserScanAggregator::SensorPoseMap sensorPoseMap;
                sensorPoseMap.reserve(sensorFrames.size());
 
                std::transform(sensorFrames.begin(),
                               sensorFrames.end(),
                               std::inserter(sensorPoseMap, sensorPoseMap.end()),
                               [&](const std::string& sensorFrame)
                               { return std::make_pair(sensorFrame, sensorPose(sensorFrame)); });
 
                aggregator = std::make_unique<LaserScanAggregator>(
                    sensorPoseMap, mapRegistration->robotPoseQueue(), properties.minLaserRange);
            }
 
            aggregator->add(laserScans);
        }
 
        ARMARX_CHECK_NOT_NULL(aggregator);
 
        ARMARX_INFO << "Point cloud size is " << aggregator->pointCloud()->size();
 
        pcl::io::savePCDFile("/home/fabi/cartographer-registration-full.pcd",
                             *aggregator->pointCloud());
 
        mapRegistration->computeBoundingBox(aggregator->pointCloud());
 
        // update the data that we have so far
        updateArViz();
 
        /************************/
 
        // mapRegistration.visualizeResult(cloud, models, combinedCorrection);
 
        // const auto slicedObjects = getSlicedObjectsFromPriorKnowledge();
 
        // const auto pointCloudData = getPointCloudsFromMemory(timeRange);
        // mapRegistration.aggregate(pointCloudData);
 
        // (2.1) "static" objects from prior knowledge + slice objects
        // (2.2) point clouds (SLAM data) from ArMem
 
        // mapRegistration.align(slicedObjects);
 
        // (3) do the magic. align to map
 
        // (4) update working memory
        // (5) update prior knowledge (create snapshot??)
 
        updateArViz();
    }
 
    Eigen::Affine3f
    CartographerMapRegistration::selectedBoundingBoxCorner() const
    {
        const armarx::cartographer::RotatedRect bb = mapRegistration->getBoundingBox();
        return bb.cornerPose(boundingBoxCorner);
    }
 
    void
    CartographerMapRegistration::updateArViz()
    {
        arvizDrawer.drawFrames(world_T_map);
 
        // update the data that we have so far
        if (mapRegistration)
        {
            const auto robotPoses = mapRegistration->robotPoses();
            arvizDrawer.drawRobotPoses(robotPoses, world_T_map);
            arvizDrawer.drawMapBoundaries(mapRegistration->getBoundingBox(), world_T_map);
            arvizDrawer.drawSelectedBoundingBoxCorner(selectedBoundingBoxCorner(), world_T_map);
        }
 
        if (aggregator)
        {
            arvizDrawer.drawPointCloud(*aggregator->pointCloud(), world_T_map);
        }
 
        arvizDrawer.drawModelAssociations(associations, world_T_map);
    }
 
    void
    CartographerMapRegistration::selectBoundingBoxCorner(int i)
    {
        ARMARX_IMPORTANT << "Selecting bounding box corner " << i;
 
        boundingBoxCorner = i % 4;
 
        updateArViz();
    }
 
    void
    CartographerMapRegistration::alignBoundingBox()
    {
        ARMARX_IMPORTANT << "Aligning using bounding box";
 
        // update prior based on bounding rect
        world_T_map = Eigen::Translation3f(-3600, -200, 0) * selectedBoundingBoxCorner().inverse();
 
        updateArViz();
    }
 
    void
    CartographerMapRegistration::findAssociations()
    {
        ARMARX_IMPORTANT << "Finding associations";
 
        // TODO(fabian.reister): this could also be "loadScene" (until drawModels())
        constexpr float widthPillars = 410.F;
        constexpr float distancePillars = 3700.F + widthPillars;
 
        const wykobi::Model pillarBack = wykobi::makeCube({-570, 3800}, widthPillars);
        const wykobi::Model pillarFront =
            wykobi::makeCube({-590, 3800 + distancePillars}, widthPillars);
 
        models = std::vector<wykobi::Model>{pillarFront, pillarBack};
 
        arvizDrawer.drawModels(models);
 
        // find matches
 
        auto clusters = mapRegistration->detectClusters(aggregator->pointCloud());
        arvizDrawer.drawClusters(clusters, world_T_map);
 
        associations = mapRegistration->matchModelsToClusters(models, clusters, world_T_map);
 
        updateArViz();
    }
 
    void
    CartographerMapRegistration::computeCoarseCorrection()
    {
        ARMARX_IMPORTANT << "Computing local coarse correction";
 
        std::vector<MapRegistration::ModelCorrection> corrections;
 
        const MapRegistration::ModelCorrection combinedCorrection =
            mapRegistration->computeCorrection(associations);
 
        world_T_map = combinedCorrection.correction.inverse();
 
        std::vector<MapRegistration::Cluster> clusters;
        std::transform(associations.begin(),
                       associations.end(),
                       std::back_inserter(clusters),
                       [](const MapRegistration::Association& assoc)
                       { return MapRegistration::Cluster(assoc.cluster); });
 
        arvizDrawer.drawClusters(clusters, world_T_map); // clear clusters
 
        updateArViz();
    }
 
    void
    CartographerMapRegistration::computeFineCorrection()
    {
        ARMARX_IMPORTANT << "Computing local correction";
 
        std::vector<MapRegistration::ModelCorrection> corrections;
 
        int j = 0;
        for (const auto& association : associations)
        {
            MapRegistration::ModelCorrection mc = mapRegistration->alignModelToCluster(
                association.model, association.cluster, world_T_map);
            corrections.push_back(mc);
 
            ARMARX_INFO << "Correction" << mc.correction.translation();
 
            j++;
        }
 
        MapRegistration::ModelCorrection combinedCorrection =
            mapRegistration->computeCombinedCorrection(models, corrections);
 
        world_T_map = world_T_map * combinedCorrection.correction.inverse();
 
        std::vector<MapRegistration::Cluster> clusters;
        std::transform(associations.begin(),
                       associations.end(),
                       std::back_inserter(clusters),
                       [](const MapRegistration::Association& assoc)
                       { return MapRegistration::Cluster(assoc.cluster); });
 
        arvizDrawer.drawClusters(clusters, world_T_map); // clear clusters
 
        updateArViz();
    }
 
    void
    CartographerMapRegistration::storeRegistrationResult()
    {
        // store data alongside the map file
        const fs::path mapPath = ArmarXDataPath::resolvePath(properties.mapPath);
 
        fs::path jsonFilename = properties.mapToLoad;
        jsonFilename.replace_extension("json");
        const fs::path jsonStorePath = mapPath / jsonFilename;
 
        ARMARX_IMPORTANT << "Storing registration result: " << jsonStorePath.string();
 
        nlohmann::json j;
        j["x"] = static_cast<float>(world_T_map.translation().x());
        j["y"] = static_cast<float>(world_T_map.translation().y());
        j["yaw"] =
            static_cast<float>(VirtualRobot::MathTools::eigen3f2rpy(world_T_map.linear()).z());
 
        std::ofstream o(jsonStorePath);
        o << j;
    }
 
} // namespace armarx::cartographer