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);
    }
 
}