ArVizDrawerMapBuilder.cpp

Go to the documentation of this file.

#include "ArVizDrawerMapBuilder.h"
 
#include <iomanip>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <SimoxUtility/iterator.h>
 
#include "ArmarXCore/core/time/CycleUtil.h"
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/core/time/forward_declarations.h>
 
#include "RobotAPI/components/ArViz/Client/Client.h"
#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/libraries/core/FramedPose.h"
 
#include "armarx/localization_and_mapping/cartographer_adapter/utils.h"
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
 
 
    constexpr int mToMM{1000};
 
    inline Eigen::Vector3f
    toMM(const Eigen::Vector3f& vec)
    {
        return vec * 1000;
    }
 
    inline Eigen::Isometry3f
    toMM(Eigen::Isometry3f pose)
    {
        pose.translation() *= 1000;
        return pose;
    }
 
    inline std::string
    nameWithId(const std::string& name, const int id, const unsigned int idWidth = 6)
    {
        std::stringstream ss;
        ss << name << "_" << std::setw(static_cast<int>(idWidth)) << std::setfill('0') << id;
        return ss.str();
    }
 
    inline std::string
    nameWithIds(const std::string& name,
                const int id,
                const int subId,
                const unsigned int idWidth = 6)
    {
        std::stringstream ss;
        ss << name << "_" << std::setw(static_cast<int>(idWidth)) << std::setfill('0') << id << "_"
           << std::setw(static_cast<int>(idWidth)) << std::setfill('0') << subId;
        return ss.str();
    }
 
    auto
    toVizColor(const ColoredMeshGrid::ColorGrid& colorGrid, const Eigen::Array2i& numCells)
    {
        std::vector<std::vector<viz::data::Color>> colors(
            numCells.x(), std::vector<viz::data::Color>(numCells.y(), viz::Color::black()));
 
        auto toVizColor = [](const ColoredMeshGrid::Color& c)
        { return viz::Color(c.x(), c.y(), c.z(), c.w()); };
 
        auto xyIter = simox::iterator::XYIndexRangeIterator({numCells.x(), numCells.y()});
        std::for_each(xyIter.begin(),
                      xyIter.end(),
                      [&](const auto& p)
                      {
                          const auto& [x, y] = p;
                          colors[x][y] = toVizColor(colorGrid[x][y]);
                      });
 
        return colors;
    }
 
    auto
    toVizPoint(ColoredMeshGrid::VertexGrid vvertices)
    {
        // convert all from [m] to [mm]
        for (auto& vv : vvertices)
        {
            for (auto& v : vv)
            {
                v *= 1000;
            }
        }
 
        return vvertices;
    }
 
    /******   ***********/
 
    ArVizDrawerMapBuilder::ArVizDrawerMapBuilder(
        armarx::viz::Client& arviz,
        ::cartographer::mapping::MapBuilderInterface& mapBuilder,
        const Eigen::Isometry3f& world_T_map) :
        arviz(arviz),
        mapBuilder(mapBuilder),
        runningTask(new RunningTask<ArVizDrawerMapBuilder>(this, &ArVizDrawerMapBuilder::run)),
        world_T_map(world_T_map)
    {
        ARMARX_TRACE;
    }
 
    void
    ArVizDrawerMapBuilder::startMappingVisu()
    {
        ARMARX_CHECK_NOT_NULL(runningTask);
        runningTask->start();
    }
 
    ArVizDrawerMapBuilder::~ArVizDrawerMapBuilder()
    {
        ARMARX_CHECK_NOT_NULL(runningTask);
        runningTask->stop();
    }
 
    void
    ArVizDrawerMapBuilder::run()
    {
        armarx::Metronome metronome{armarx::Duration::MilliSeconds(1000)};
 
        while (!runningTask->isStopped())
        {
            drawOnce();
            metronome.waitForNextTick();
        }
    }
 
    void
    ArVizDrawerMapBuilder::drawOnce()
    {
        ARMARX_VERBOSE << "Updating visualization";
        drawFrames();
        // drawOptimizedPoseGraph();
        drawGridMap();
    }
 
    void
    ArVizDrawerMapBuilder::drawFrames()
    {
        auto framesLayer = arviz.layer("frames");
 
        const auto mapPose =
            viz::Pose(MapFrame).pose(world_T_map.translation(), world_T_map.linear()).scale(5.F);
        const auto worldPose = viz::Pose(GlobalFrame).scale(5.F);
 
        const auto arr = viz::Arrow(GlobalFrame + "/" + MapFrame)
                             .fromTo(Eigen::Vector3f::Zero(), world_T_map.translation());
 
        framesLayer.add(worldPose);
        framesLayer.add(mapPose);
        framesLayer.add(arr);
        arviz.commit(framesLayer);
    }
 
    void
    ArVizDrawerMapBuilder::drawOptimizedPoseGraph()
    {
        auto optimizedGraphLayerGlobal = arviz.layer("optimized_graph_global");
 
        const auto optimizedGraphData = utils::optimizedGraphData(mapBuilder);
 
        for (const auto& nodeData : optimizedGraphData.nodes_data)
        {
            const Eigen::Isometry3f globalPose = world_T_map * toMM(nodeData.global_pose);
 
            const auto globalPoseViz = viz::Pose("node_" + std::to_string(nodeData.node_id))
                                           .position(globalPose.translation())
                                           .orientation(globalPose.linear());
            optimizedGraphLayerGlobal.add(globalPoseViz);
        }
 
        arviz.commit(optimizedGraphLayerGlobal);
    }
 
    void
    ArVizDrawerMapBuilder::drawGridMap()
    {
        const SubMapDataVector submapData = utils::submapData(mapBuilder, 0); // TODO
 
        std::map<int, viz::Layer> layers;
 
        if (submapData.empty())
        {
            return;
        }
 
        for (const auto& submap : submapData)
        {
            if (layers.find(submap.trajectory_id) == layers.end())
            {
                layers[submap.trajectory_id] =
                    arviz.layer(nameWithId("grid_maps_trajectory", submap.trajectory_id));
            }
 
            auto& gridMapLayer = layers.at(submap.trajectory_id);
 
            auto mesh = viz::Mesh(nameWithIds("submap", submap.trajectory_id, submap.id));
 
            const Eigen::Isometry3f mapPose = toMM(submap.global_pose());
            const Eigen::Isometry3f globalPose = world_T_map * mapPose;
 
            // ARMARX_DEBUG << "arviz drawer: submap pose " << submapPose.matrix();
 
            mesh.position(globalPose.translation() + Eigen::Vector3f{0.F, 0.F, -1.F})
                .orientation(globalPose.linear())
                .grid2D(toVizPoint(submap.mesh_grid.vertices),
                        toVizColor(submap.mesh_grid.colors, submap.mesh_grid.num_cells));
 
            gridMapLayer.add(mesh);
        }
 
        const auto layersV = simox::alg::get_values(layers);
        arviz.commit(layersV);
    }
 
} // namespace armarx::localization_and_mapping::cartographer_adapter