CoinPointCloud.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-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/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "CoinPointCloud.h"
 
#include <vector>
 
//Coin includes
#include <pcl/common/colors.h>
 
#include <Inventor/SbColor.h>
#include <Inventor/SbVec3f.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoPointSet.h>
 
namespace visionx
{
 
    template <class ColorT>
    void
    fillColorArray(float array[3], const ColorT& color)
    {
        array[0] = static_cast<float>(color.r) / 255.0f;
        array[1] = static_cast<float>(color.g) / 255.0f;
        array[2] = static_cast<float>(color.b) / 255.0f;
    }
 
    CoinPointCloud::CoinPointCloud(pcl::PointCloud<PointT>::ConstPtr originalCloud,
                                   visionx::PointContentType originalType,
                                   bool colorFromLabel,
                                   float pointSize) :
        originalCloud(originalCloud), originalType(originalType)
    {
        // Insert color information into scene graph
        SoMaterial* materialInfo = new SoMaterial();
        std::vector<SbColor> colorData;
        colorData.reserve(originalCloud->points.size());
        // Add point coordinates
        SoCoordinate3* coordinates = new SoCoordinate3();
        std::vector<SbVec3f> pointData;
        pointData.reserve(originalCloud->points.size());
        for (const PointT& p : originalCloud->points)
        {
            if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z))
            {
                SbColor colorContainer;
                float colorArray[3];
 
                if (colorFromLabel)
                {
                    const pcl::RGB color = pcl::GlasbeyLUT::at(p.label % pcl::GlasbeyLUT::size());
                    fillColorArray(colorArray, color);
                }
                else
                {
                    fillColorArray(colorArray, p);
                }
 
                colorContainer.setValue(colorArray);
                colorData.push_back(colorContainer);
 
                SbVec3f pointContainer;
                pointContainer[0] = p.x;
                pointContainer[1] = p.y;
                pointContainer[2] = p.z;
 
                pointData.push_back(pointContainer);
            }
        }
 
        materialInfo->diffuseColor.setValues(0, colorData.size(), colorData.data());
        this->addChild(materialInfo);
 
        // Bind materials to per part
        SoMaterialBinding* binding = new SoMaterialBinding();
        binding->value = SoMaterialBinding::PER_PART;
        this->addChild(binding);
 
        coordinates->point.setValues(0, pointData.size(), pointData.data());
        this->addChild(coordinates);
 
        // Set point size
        SoDrawStyle* sopointSize = new SoDrawStyle();
        sopointSize->pointSize = pointSize;
        this->addChild(sopointSize);
 
        // Draw a point set out of all that data
        SoPointSet* pointSet = new SoPointSet();
        this->addChild(pointSet);
    }
 
    CoinPointCloud::~CoinPointCloud() = default;
 
    auto
    CoinPointCloud::getPCLCloud() const -> pcl::PointCloud<PointT>::ConstPtr
    {
        return this->originalCloud;
    }
 
    visionx::PointContentType
    CoinPointCloud::getOriginalType() const
    {
        return this->originalType;
    }
} // namespace visionx