ArVizDrawer.cpp

Go to the documentation of this file.

#include "ArVizDrawer.h"
// #include "RobotComponents/components/CartographerMapRegistration/ArVizDrawer.h"
 
#include <algorithm>
#include <string>
 
#include "RobotAPI/components/ArViz/Client/Client.h"
#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/Layer.h"
#include "RobotAPI/components/ArViz/Client/elements/Color.h"
#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
#include "RobotAPI/libraries/core/FramedPose.h"
 
#include "RobotComponents/libraries/cartographer/map_registration/wykobi_types.h"
 
namespace armarx::cartographer
{
    namespace detail
    {
        inline Eigen::Vector3f
        toEigen(const pcl::PointXYZ& pt)
        {
            return Eigen::Vector3f{pt.x, pt.y, pt.z};
        }
    } // namespace detail
 
    ArVizDrawer::ArVizDrawer(armarx::viz::Client& arviz) : ArVizDrawerBase(arviz)
    {
        // clear(); boom...
    }
 
    ArVizDrawer::~ArVizDrawer()
    {
        clear();
    }
 
    void
    ArVizDrawer::clear()
    {
        const auto clearLayer = [&](const std::string& layerName)
        {
            auto layer = arviz().layer(layerName);
            commit(layer);
        };
 
        clearLayer(layers.mapBoundaries);
        clearLayer(layers.modelSceneAssociations);
        clearLayer(layers.sceneModels);
        clearLayer(layers.robotPoses);
        clearLayer(layers.selectedBoundingBoxCorner);
        clearLayer(layers.clusters);
        clearLayer(layers.pointCloud);
        clearLayer(layers.frames);
    }
 
    void
    ArVizDrawer::drawFrames(const Eigen::Affine3f& world_T_map)
    {
        auto framesLayer = arviz().layer(layers.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, mapPose, arr);
        commit(framesLayer);
    }
 
    void
    ArVizDrawer::drawCluster(const MapRegistration::Cluster& cluster,
                             const Eigen::Affine3f& world_T_map,
                             viz::Layer& layer) const
    {
        const size_t id = layer.size();
 
        // cluster: point cloud
        const std::string clusterPointsName = "cluster_points_" + std::to_string(id);
 
        auto pc = viz::PointCloud(clusterPointsName)
                      .pose(world_T_map.translation(), world_T_map.linear());
        pc.pointSizeInPixels(10).pointCloud(*cluster.points, viz::Color::gray(200));
 
        layer.add(pc);
 
        // cluster: centroid
        const std::string clusterCentroidName = "centroid_" + std::to_string(id);
 
        Eigen::Vector3f world_P_cluster = world_T_map * detail::toEigen(cluster.centroid);
 
        auto pose = viz::Pose(clusterCentroidName).position(world_P_cluster);
        layer.add(pose);
    }
 
    void
    ArVizDrawer::drawPointCloud(const MapRegistration::PointCloud& cloud,
                                const Eigen::Affine3f& world_T_map)
    {
        auto layer = arviz().layer(layers.pointCloud);
 
        // cluster: point cloud
        const std::string clusterPointsName = "point_cloud";
 
        auto pc = viz::PointCloud(clusterPointsName)
                      .pose(world_T_map.translation(), world_T_map.linear());
        pc.pointSizeInPixels(5).pointCloud(cloud.points, viz::Color::gray(100));
 
        layer.add(pc);
 
        commit(layer);
    }
 
    void
    ArVizDrawer::drawClusters(const MapRegistration::Clusters& clusters,
                              const Eigen::Affine3f& world_T_map)
    {
        auto layer = arviz().layer(layers.clusters);
 
        std::for_each(clusters.begin(),
                      clusters.end(),
                      [&](const auto& cluster) { drawCluster(cluster, world_T_map, layer); });
 
        commit(layer);
    }
 
    void
    ArVizDrawer::drawSelectedBoundingBoxCorner(const Eigen::Affine3f& boundingBoxCornerPose,
                                               const Eigen::Affine3f& world_T_map)
    {
        auto layer = arviz().layer(layers.selectedBoundingBoxCorner);
 
        const Eigen::Affine3f world_T_corner = world_T_map * boundingBoxCornerPose;
        auto pose = viz::Pose("box_corner")
                        .pose(world_T_corner.translation(), world_T_corner.linear())
                        .scale(3.F);
 
        layer.add(pose);
 
        commit(layer);
    }
 
    void
    ArVizDrawer::drawRobotPose(const Eigen::Affine3f& robotPose,
                               const Eigen::Affine3f& world_T_map,
                               viz::Layer& layer) const
    {
        const size_t id = layer.size();
 
        // cluster: point cloud
        const std::string robotPoseName = "robot_pose_" + std::to_string(id);
 
        const Eigen::Affine3f world_T_robot = world_T_map * robotPose; // TODO
 
        auto pose =
            viz::Pose(robotPoseName).pose(world_T_robot.translation(), world_T_robot.linear());
        layer.add(pose);
    }
 
    void
    ArVizDrawer::drawRobotPoses(const std::vector<Eigen::Affine3f>& robotPoses,
                                const Eigen::Affine3f& world_T_map)
    {
        auto layer = arviz().layer(layers.robotPoses);
 
        std::for_each(robotPoses.begin(),
                      robotPoses.end(),
                      [&](const auto& robotPose) { drawRobotPose(robotPose, world_T_map, layer); });
 
        commit(layer);
    }
 
    void
    ArVizDrawer::drawModel(const wykobi::Model& model, viz::Layer& layer) const
    {
        const size_t id = layer.size();
        const std::string polygonModelName = "model_polygon_" + std::to_string(id);
 
        auto py = viz::Polygon(polygonModelName).color(viz::Color::azure());
 
        std::for_each(model.polygon.begin(),
                      model.polygon.end(),
                      [&py](const wykobi::Point& point) {
                          py.addPoint(Eigen::Vector3f{point.x, point.y, -4.F});
                      });
 
        layer.add(py);
    }
 
    void
    ArVizDrawer::drawModels(const std::vector<wykobi::Model>& models)
    {
        auto layer = arviz().layer(layers.sceneModels);
 
        std::for_each(
            models.begin(), models.end(), [&](const auto& model) { drawModel(model, layer); });
 
        commit(layer);
    }
 
    void
    ArVizDrawer::drawModelAssociation(const MapRegistration::Association& association,
                                      const Eigen::Affine3f& world_T_map,
                                      viz::Layer& layer) const
    {
        const size_t id = layer.size();
        const std::string associationName = "association_" + std::to_string(id);
 
        const Eigen::Vector3f from = world_T_map * detail::toEigen(association.cluster.centroid);
        const Eigen::Vector3f to = association.model.center;
 
        const auto arr = viz::Arrow(associationName).fromTo(from, to);
 
        layer.add(arr);
 
        // TODO(fabian.reister): draw clusters e.g. in yellow
    }
 
    void
    ArVizDrawer::drawModelAssociations(
        const std::vector<MapRegistration::Association>& associations,
        const Eigen::Affine3f& world_T_map)
    {
        auto layer = arviz().layer(layers.modelSceneAssociations);
 
        std::for_each(associations.begin(),
                      associations.end(),
                      [&](const auto& association)
                      { drawModelAssociation(association, world_T_map, layer); });
 
        commit(layer);
    }
 
    void
    ArVizDrawer::drawMapBoundaries(const RotatedRect& boundingRect,
                                   const Eigen::Affine3f& world_T_map)
    {
        auto layer = arviz().layer(layers.mapBoundaries);
 
        auto ply = viz::Polygon("map_boundaries");
 
        const auto boundingPts = boundingRect.boundary();
 
        const auto toEigen = [](const Eigen::Vector2f& pt) -> Eigen::Vector3f {
            return Eigen::Vector3f{pt.x(), pt.y(), -10.F};
        };
 
        for (const auto& pt : boundingPts)
        {
            const Eigen::Vector3f world_P_pt = world_T_map * toEigen(pt);
            ply.addPoint(world_P_pt);
        }
 
        layer.add(ply);
 
        commit(layer);
    }
 
 
} // namespace armarx::cartographer