VisualizationPointCloud.h

Go to the documentation of this file.

#pragma once
 
#include "ElementVisualizer.h"
 
#include <RobotAPI/interface/ArViz/Elements.h>
 
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoPointSet.h>
 
#include <x86intrin.h>
 
namespace armarx::viz::coin
{
    struct VisualizationPointCloud : TypedElementVisualization<SoSeparator>
    {
        using ElementType = data::ElementPointCloud;
 
        VisualizationPointCloud()
        {
            pclMat = new SoMaterial;
 
            SoMaterialBinding* pclMatBind = new SoMaterialBinding;
            pclMatBind->value = SoMaterialBinding::PER_PART;
 
            pclCoords = new SoCoordinate3;
            pclStyle = new SoDrawStyle;
 
            node->addChild(pclMat);
            node->addChild(pclMatBind);
            node->addChild(pclCoords);
            node->addChild(pclStyle);
            node->addChild(new SoPointSet);
        }
 
        __attribute__((target("default")))
        bool update(ElementType const& element)
        {
            data::ColoredPoint const* pclData = (data::ColoredPoint const*)element.points.data();
            int pclSize = element.points.size() / sizeof(data::ColoredPoint);
 
            int singleBufferSize = pclSize * 3;
            buffer.resize(singleBufferSize * 2);
 
            const float conv = 1.0f / 255.0f;
            float* coordsData = buffer.data();
            float* colorsData = buffer.data() + singleBufferSize;
 
            for (int i = 0; i < pclSize; ++i)
            {
                data::ColoredPoint point = pclData[i];
                colorsData[i * 3 + 0] = point.color.r * conv;
                colorsData[i * 3 + 1] = point.color.g * conv;
                colorsData[i * 3 + 2] = point.color.b * conv;
 
                coordsData[i * 3 + 0] = point.x;
                coordsData[i * 3 + 1] = point.y;
                coordsData[i * 3 + 2] = point.z;
            }
 
            pclMat->diffuseColor.setValuesPointer(pclSize, colorsData);
            pclMat->ambientColor.setValuesPointer(pclSize, colorsData);
            pclMat->transparency = element.transparency;
 
            pclCoords->point.setValuesPointer(pclSize, coordsData);
 
            pclStyle->pointSize = element.pointSizeInPixels;
 
            return true;
        }
 
        __attribute__((target("sse4.1")))
        bool update(ElementType const& element)
        {
            float* pclIn = (float*)element.points.data();
            int pclSize = element.points.size() / sizeof(data::ColoredPoint);
 
            // Enlarge and align the buffers
            int singleBufferSize = (pclSize + 3) * 3;
            if (singleBufferSize % 16 != 0)
            {
                singleBufferSize += 16 - singleBufferSize % 16;
            }
            buffer.resize(singleBufferSize * 2);
 
            float* positionsData = buffer.data();
            float* colorsData = buffer.data() + singleBufferSize;
 
            float* colorsOut = colorsData;
            float* positionsOut = positionsData;
 
            __m128 convColor = _mm_set1_ps(1.0f / 255.0f);
            __m128i offsetBase = _mm_set_epi32(12, 8, 4, 0);
            __m128i offsetIncrement = _mm_set1_epi32(16);
            __m128i maxOffset = _mm_set1_epi32(4 * (pclSize - 1));
            __m128 floatMax = _mm_set1_ps(FLT_MAX);
 
 
            //            std::uint64_t timerStart = __rdtsc();
            // Work on four points at a time
            for (int i = 0; i < pclSize; i += 4)
            {
                // Ensure that the offsets do not exceed the input size
                __m128i offsets = _mm_min_epi32(offsetBase, maxOffset);
 
                // Load four colored point
                // Memory layout of a colored point: c z y x
                __m128 cp0 = _mm_loadu_ps(pclIn + _mm_extract_epi32(offsets, 0));
                __m128 cp1 = _mm_loadu_ps(pclIn + _mm_extract_epi32(offsets, 1));
                __m128 cp2 = _mm_loadu_ps(pclIn + _mm_extract_epi32(offsets, 2));
                __m128 cp3 = _mm_loadu_ps(pclIn + _mm_extract_epi32(offsets, 3));
 
                // Shift the colored point data, so that we can blend them together
                // 0 -> x, 1 -> y, 2 -> z, 3 -> c
                // shiftedP0: c z y x
                //__m128i shiftedP0 = _mm_shuffle_epi32((__m128i)cp0, _MM_SHUFFLE(3, 2, 1, 0));
                __m128i shiftedP0 = (__m128i)cp0;
                // shiftedP1: x c z y
                __m128i shiftedP1 = _mm_shuffle_epi32((__m128i)cp1, _MM_SHUFFLE(0, 3, 2, 1));
                // shiftedP2: y x c z
                __m128i shiftedP2 = _mm_shuffle_epi32((__m128i)cp2, _MM_SHUFFLE(1, 0, 3, 2));
                // shiftedP3: z y x c
                __m128i shiftedP3 = _mm_shuffle_epi32((__m128i)cp3, _MM_SHUFFLE(2, 1, 0, 3));
 
                // Blend together entries from two colored points to gather the packed position data
                // [x] (P1) [z y x] (P0)  ==> 1000 = 8
                __m128 p0 = _mm_blend_ps((__m128)shiftedP0, (__m128)shiftedP1, 8);
                // [y x] (P2)  [z y] (P1) ==> 1100 = 12
                __m128 p1 = _mm_blend_ps((__m128)shiftedP1, (__m128)shiftedP2, 12);
                // [z y x] (P3) [z] (P2)  ==> 1110 = 14
                __m128 p2 = _mm_blend_ps((__m128)shiftedP2, (__m128)shiftedP3, 14);
 
                // Replace NaNs with zeros
                p0 = _mm_and_ps(p0, _mm_cmplt_ps(p0, floatMax));
                p1 = _mm_and_ps(p1, _mm_cmplt_ps(p1, floatMax));
                p2 = _mm_and_ps(p2, _mm_cmplt_ps(p2, floatMax));
 
                // Store the position data (3 registers, 4 position values)
                _mm_storeu_ps(positionsOut + 0, p0);
                _mm_storeu_ps(positionsOut + 4, p1);
                _mm_storeu_ps(positionsOut + 8, p2);
 
                // Blend together the entries from all four colored points to gather packed color data
                // [c] (P0) [c z y] (P1) ==> 0111 ==> 7
                __m128 c01 = _mm_blend_ps((__m128)shiftedP0, (__m128)shiftedP1, 7);
                // [y x c] (P2) [c] (P3) ==> 0001 ==> 1
                __m128 c23 = _mm_blend_ps((__m128)shiftedP2, (__m128)shiftedP3, 1);
                // [c c] (c01) [c c] (c23) ==> 0011 ==> 3
                __m128i c0123 = _mm_castps_si128(_mm_blend_ps(c01, c23, 3));
 
                // Extract the color channels from the packed color data
                // c = b g r a (8-bit each)
                // Range [0, 255]
                __m128i redI = _mm_and_si128(_mm_bsrli_si128(c0123, 1), _mm_set1_epi32(0xFF));
                __m128i greenI = _mm_and_si128(_mm_bsrli_si128(c0123, 2), _mm_set1_epi32(0xFF));
                __m128i blueI = _mm_and_si128(_mm_bsrli_si128(c0123, 3), _mm_set1_epi32(0xFF));
 
                // Convert the integer color channels to float channels
                // Range [0.0, 1.0]
                __m128 red = _mm_mul_ps(_mm_cvtepi32_ps(redI), convColor);
                __m128 green = _mm_mul_ps(_mm_cvtepi32_ps(greenI), convColor);
                __m128 blue = _mm_mul_ps(_mm_cvtepi32_ps(blueI), convColor);
 
                // Construct the output memory order (4 color values in 3 registers)
                // [r g b] [r
                //  g b] [r g
                // b] [r g b]
                __m128 c0 = _mm_setr_ps(red[3], green[3], blue[3], red[2]);
                __m128 c1 = _mm_setr_ps(green[2], blue[2], red[1], green[1]);
                __m128 c2 = _mm_setr_ps(blue[1], red[0], green[0], blue[0]);
 
                // Store the color data (3 registers, 4 color values)
                _mm_storeu_ps(colorsOut + 0, c0);
                _mm_storeu_ps(colorsOut + 4, c1);
                _mm_storeu_ps(colorsOut + 8, c2);
 
                // Advance the output pointers by 4 values (3 floats per position/color)
                colorsOut += 12;
                positionsOut += 12;
 
                // Move the input offsets to the next 4 colored points
                offsetBase = _mm_add_epi32(offsetBase, offsetIncrement);
            }
            //            std::uint64_t timerEnd = __rdtsc();
            //            int timerDiff = (int)(timerEnd - timerStart);
            //            float ticksPerPoint = (float)timerDiff / pclSize;
            //            printf("%c[2KUpdate Time %d\tT/Point %g\n", 27, timerDiff, ticksPerPoint);
 
            pclMat->diffuseColor.setValuesPointer(pclSize, colorsData);
            pclMat->ambientColor.setValuesPointer(pclSize, colorsData);
            pclMat->transparency = element.transparency;
 
            pclCoords->point.setValuesPointer(pclSize, positionsData);
 
            pclStyle->pointSize = element.pointSizeInPixels;
 
            return true;
        }
 
        SoMaterial* pclMat;
        SoCoordinate3* pclCoords;
        SoDrawStyle* pclStyle;
 
        std::vector<SbColor> colors;
        std::vector<SbVec3f> coords;
 
        std::vector<float> buffer;
    };
}