Visualizer.hpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
 * Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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/>.
 *
 * @author     Rainer Kartmann (rainer dot kartmann at student dot kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <thread>
 
#include <Eigen/Geometry>
 
#include <VirtualRobot/math/Helpers.h>
 
#include <RobotAPI/libraries/core/visualization/DebugDrawerTopic.h>
 
#include "VoxelGrid.hpp"
 
namespace visionx::voxelgrid
{
 
    /**
     * Visualizer for voxel grids of with voxel type VoxelT.
     */
    template <typename VoxelT>
    class Visualizer
    {
    public:
        /// Default constructor.
        Visualizer();
        /// Initialize with debug drawer and voxel grid.
        Visualizer(const armarx::DebugDrawerTopic& drawer,
                   const std::string& voxelLayer = "VoxelGrid");
 
        /// Virtual destructor.
        virtual ~Visualizer()
        {
        }
 
        /// Clear the layer.
        void clearLayer();
        /// Clear the voxel layer.
        void clearVoxelLayer(bool sleep = false);
 
        /// Draw the voxel grid.
        void draw(const VoxelGrid<VoxelT>& grid);
 
        /// Draw the bounding edges of the voxel grid.
        void drawBoundingEdges(const VoxelGrid<VoxelT>& grid);
 
        /// Draw the bounding edges of the voxel grid.
        void drawStructure(const VoxelGrid<VoxelT>& grid,
                           float lineWidth = 1,
                           const armarx::DrawColor& colorEdges = {0, .5, 1, 1},
                           const armarx::DrawColor& colorStructure = {0, 1, 1, 1},
                           bool drawOriginVoxel = false);
 
        /// Set the debug drawer.
        void
        setDebugDrawer(const armarx::DebugDrawerInterfacePrx& debugDrawer)
        {
            this->_debugDrawer = debugDrawer;
        }
 
        /// Set the layer name.
        void
        setLayer(const std::string& layer)
        {
            this->_layer = layer;
        }
 
        operator bool() const
        {
            return _debugDrawer.operator bool();
        }
 
 
    protected:
        /// Information about a voxel about to-be-drawn.
        struct VoxelVisuData
        {
            const VoxelGrid<VoxelT>& grid; /// The voxel grid.
            const std::string& name; /// A unique visu name for this voxel.
 
            std::size_t index; /// The voxel's index.
            const VoxelT& voxel; /// The voxel.
 
            const Eigen::Vector3f& pos; /// The voxel's position.
            const Eigen::Quaternionf& ori; /// The voxel's orientation.
            const Eigen::Vector3f& extents; /// The voxel extents.
        };
 
        /// Indicate whether a voxel shall be drawn.
        /// Voxels for which this method returns false are not passed to drawVoxel().
        virtual bool
        isVisible(const VoxelVisuData& visu) const
        {
            (void)visu;
            return true;
        }
 
        /// Draw a voxel.
        virtual void drawVoxel(const VoxelVisuData& visu);
 
        /// Get the debug drawer.
        armarx::DebugDrawerTopic&
        drawer()
        {
            return _debugDrawer;
        }
 
        const armarx::DebugDrawerTopic&
        drawer() const
        {
            return _debugDrawer;
        }
 
        /// Get the layer name.
        std::string
        getVoxelLayer() const
        {
            return _layer + "_voxels";
        }
 
 
    private:
        /// The debug drawer.
        armarx::DebugDrawerTopic _debugDrawer;
 
        /// The layer name.
        std::string _layer;
    };
 
    // IMPLEMENTATION
 
 
    template <typename VT>
    Visualizer<VT>::Visualizer()
    {
    }
 
    template <typename VT>
    Visualizer<VT>::Visualizer(const armarx::DebugDrawerTopic& debugDrawer,
                               const std::string& layer) :
        _debugDrawer(debugDrawer), _layer(layer)
    {
        using namespace std::chrono_literals;
        _debugDrawer.setLayer(layer);
        _debugDrawer.setShortSleepDuration(100ms);
    }
 
    template <typename VT>
    void
    Visualizer<VT>::clearLayer()
    {
        _debugDrawer.clearLayer(_layer);
    }
 
    template <typename VT>
    void
    Visualizer<VT>::clearVoxelLayer(bool sleep)
    {
        _debugDrawer.clearLayer(getVoxelLayer(), sleep);
    }
 
    template <typename VT>
    void
    Visualizer<VT>::drawBoundingEdges(const VoxelGrid<VT>& grid)
    {
        if (!_debugDrawer)
        {
            return;
        }
 
        std::vector<Eigen::Vector3f> points;
        float width = 0.025f;
        armarx::DrawColor color{0, 1, 1, 1};
 
        const Eigen::Matrix32f bb = grid.getLocalBoundingBox();
        const Eigen::Matrix4f pose = grid.getPose();
 
        auto addLine = [&](int x1, int y1, int z1, int x2, int y2, int z2)
        {
            Eigen::Vector3f start = {bb.col(x1).x(), bb.col(y1).y(), bb.col(z1).z()};
            Eigen::Vector3f end = {bb.col(x2).x(), bb.col(y2).y(), bb.col(z2).z()};
 
            start = math::Helpers::TransformPosition(pose, start);
            end = math::Helpers::TransformPosition(pose, end);
 
            points.push_back({start.x(), start.y(), start.z()});
            points.push_back({end.x(), end.y(), end.z()});
        };
 
        /*   001 +-----+ 011
         *      /| 111/|
         * 101 +-----+ |
         *     | +---|-+ 010
         *     |/000 |/
         * 100 +-----+ 110
         */
 
        // 000 -> 100, 010, 001
        addLine(0, 0, 0, 1, 0, 0);
        addLine(0, 0, 0, 0, 1, 0);
        addLine(0, 0, 0, 0, 0, 1);
 
        // 111 -> 011, 101, 110
        addLine(1, 1, 1, 0, 1, 1);
        addLine(1, 1, 1, 1, 0, 1);
        addLine(1, 1, 1, 1, 1, 0);
 
        // 100 -> 101, 110
        addLine(1, 0, 0, 1, 0, 1);
        addLine(1, 0, 0, 1, 1, 0);
 
        // 010 -> 110, 011
        addLine(0, 1, 0, 1, 1, 0);
        addLine(0, 1, 0, 0, 1, 1);
 
        // 001 -> 101, 011
        addLine(0, 0, 1, 1, 0, 1);
        addLine(0, 0, 1, 0, 1, 1);
 
        _debugDrawer.drawLineSet({_layer, "edges"}, points, width, color);
    }
 
    template <typename VT>
    void
    Visualizer<VT>::drawStructure(const VoxelGrid<VT>& grid,
                                  float lineWidth,
                                  const armarx::DrawColor& colorEdges,
                                  const armarx::DrawColor& colorStructure,
                                  bool drawOriginVoxel)
    {
        if (!_debugDrawer)
        {
            return;
        }
 
        armarx::DebugDrawerLineSet lines;
        lines.lineWidth = lineWidth;
        lines.colorNoIntensity = colorStructure;
        lines.colorFullIntensity = colorEdges;
 
        const Eigen::Matrix32f bb = grid.getLocalBoundingBox();
        const Eigen::Matrix4f pose = grid.getPose();
 
        auto addLine = [&](const Eigen::Vector3f& startLocal,
                           const Eigen::Vector3f& endLocal,
                           float intensity = 1)
        {
            const Eigen::Vector3f start = math::Helpers::TransformPosition(pose, startLocal);
            const Eigen::Vector3f end = math::Helpers::TransformPosition(pose, endLocal);
 
            lines.points.push_back({start.x(), start.y(), start.z()});
            lines.points.push_back({end.x(), end.y(), end.z()});
 
            lines.intensities.push_back(intensity);
        };
 
        auto addEdge = [&](int x1, int y1, int z1, int x2, int y2, int z2)
        {
            const Eigen::Vector3f start = {bb.col(x1).x(), bb.col(y1).y(), bb.col(z1).z()};
            const Eigen::Vector3f end = {bb.col(x2).x(), bb.col(y2).y(), bb.col(z2).z()};
            addLine(start, end, 1);
        };
 
        /*   001 +-----+ 011
         *      /| 111/|
         * 101 +-----+ |
         *     | +---|-+ 010
         *     |/000 |/
         * 100 +-----+ 110
         */
 
        // 000 -> 100, 010, 001
        addEdge(0, 0, 0, 1, 0, 0);
        addEdge(0, 0, 0, 0, 1, 0);
        addEdge(0, 0, 0, 0, 0, 1);
 
        // 111 -> 011, 101, 110
        addEdge(1, 1, 1, 0, 1, 1);
        addEdge(1, 1, 1, 1, 0, 1);
        addEdge(1, 1, 1, 1, 1, 0);
 
        // 100 -> 101, 110
        addEdge(1, 0, 0, 1, 0, 1);
        addEdge(1, 0, 0, 1, 1, 0);
 
        // 010 -> 110, 011
        addEdge(0, 1, 0, 1, 1, 0);
        addEdge(0, 1, 0, 0, 1, 1);
 
        // 001 -> 101, 011
        addEdge(0, 0, 1, 1, 0, 1);
        addEdge(0, 0, 1, 0, 1, 1);
 
        const auto min = bb.col(0);
        const auto max = bb.col(1);
 
        auto addQuad = [&](const Eigen::Vector3f& center, int h1, int h2, float intensity = 0)
        {
            /* -/+ D----^----C +/+
             *     |  h2|    |
             *     |    +---->
             *     |   c  h1 |
             * -/- A---------B +/-
             */
 
            Eigen::Vector3f A, B, C, D;
            A = B = C = D = center;
 
            A(h1) = min(h1);
            A(h2) = min(h2);
 
            B(h1) = max(h1);
            B(h2) = min(h2);
 
            C(h1) = max(h1);
            C(h2) = max(h2);
 
            D(h1) = min(h1);
            D(h2) = max(h2);
 
            addLine(A, B, intensity);
            addLine(B, C, intensity);
            addLine(C, D, intensity);
            addLine(D, A, intensity);
        };
 
 
        Eigen::Vector3i index = index.Zero();
        Eigen::Vector3f center = center.Zero();
 
        const Eigen::Vector3i indicesMinMax[2] = {grid.getVoxelGridIndexMin(),
                                                  grid.getVoxelGridIndexMax()};
 
        for (int i = 0; i < 3; ++i)
        {
            int h1 = (i + 1) % 3;
            int h2 = (i + 2) % 3;
 
            for (int mm : {0, 1}) // min / max
            {
                index(i) = indicesMinMax[mm](i);
                center(i) = grid.getVoxelCenter(index, true)(i);
 
                // Shift: min => -size,  max => +size
                float shift = (mm == 0 ? -1 : +1) * grid.getVoxelSizes()(i);
 
                // Add quad at bounding edge (shift one voxel to center).
                // (min => +shift,  max => -shift)
                center(i) -= shift;
                addQuad(center, h1, h2);
 
                // Add quad at origin (at +- size/2 around origin).
                center(i) = shift / 2;
                addQuad(center, h1, h2);
            }
            if (grid.getGridSizes()(i) % 2 == 0)
            {
                // Also add a quad at origin for min of voxels at -1.
                center(i) = -1.5 * grid.getVoxelSizes()(i);
                addQuad(center, h1, h2);
            }
        }
 
        _debugDrawer.drawLineSet({_layer, "structure"}, lines);
 
        if (drawOriginVoxel)
        {
            _debugDrawer.drawBox({_layer, "origin_voxel"},
                                 grid.getOrigin(),
                                 grid.getOrientation(),
                                 grid.getVoxelSizes(),
                                 _debugDrawer.toDrawColor(colorEdges, .25));
        }
    }
 
    template <typename VoxelT>
    void
    Visualizer<VoxelT>::draw(const VoxelGrid<VoxelT>& grid)
    {
        if (!_debugDrawer)
        {
            return;
        }
 
        for (std::size_t i = 0; i < grid.numVoxels(); ++i)
        {
            const VoxelT& voxel = grid.getVoxels()[i];
            const Eigen::Vector3f center = grid.getVoxelCenter(i);
 
            VoxelVisuData visu{grid,
                               "voxel_" + std::to_string(i),
                               i,
                               voxel,
                               center,
                               grid.getOrientation(),
                               grid.getVoxelSizes()};
 
            if (isVisible(visu))
            {
                drawVoxel(visu);
            }
        }
    }
 
    template <typename VoxelT>
    void
    Visualizer<VoxelT>::drawVoxel(const VoxelVisuData& visu)
    {
        const armarx::DrawColor color{.5, .5, .5, 1};
        drawer().drawBox({getVoxelLayer(), visu.name}, visu.pos, visu.ori, visu.extents, color);
    }
 
} // namespace visionx::voxelgrid