mlPCLInventorTools.h

Go to the documentation of this file.

// Copyright (c) Fraunhofer MEVIS, Germany. All rights reserved.
// **InsertLicense** code author="Wolf Spindler"
//----------------------------------------------------------------------------------
 
//----------------------------------------------------------------------------------
#pragma once
 
#include "MLPCLInventorAdaptersSystem.h"
#include <mlPCLTypes.h>
#include <mlPCLSupportTools.h>
#include <mlErrorMacros.h>
#include <mlRangeCasts.h>
#include <PCLThirdPartyWarningsDisable.h>
#include <Inventor/nodes/SoVertexProperty.h>
#include <PCLThirdPartyWarningsRestore.h>
 
// Forwards to internally used classes.
class SoVertexProperty;
class SoMFInt32;
 
ML_START_NAMESPACE
 
namespace PCLInventorTools {
 
//----------------------------------------------------------------------------------
// DESTINATION: SbVec2f
//----------------------------------------------------------------------------------
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void setOneMFVecFieldValue(SbVec2f *dstVal, const float data[4])
{
  dstVal->setValue(data[0], data[1]);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void setOneMFVecFieldValue(SbVec2f *dstVal, const unsigned int * /*rgbaVal*/)
{
  dstVal->setValue(0.f, 0.f);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void setOneMFVecFieldValue(SbVec2f *dstVal, float data)
{
  dstVal->setValue(data, data);
}
 
 
//----------------------------------------------------------------------------------
// DESTINATION: SbVec3f
//----------------------------------------------------------------------------------
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void setOneMFVecFieldValue(SbVec3f *dstVal, const float data[4])
{
  dstVal->setValue(data[0], data[1], data[2]);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void setOneMFVecFieldValue(SbVec3f *dstVal, const unsigned int * /*rgbaVal*/)
{
  dstVal->setValue(0.f, 0.f, 0.f);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void setOneMFVecFieldValue(SbVec3f *dstVal, float data)
{
  dstVal->setValue(data, data, data);
}
 
 
 
//----------------------------------------------------------------------------------
// DESTINATION: MLuint32
//----------------------------------------------------------------------------------
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void setOneMFVecFieldValue(MLuint32 *dstVal, const float data)
{
  const MLuint32 byteVal = static_cast<MLuint32>(data);
  *dstVal = (byteVal | (byteVal <<  8) | (byteVal << 16) | (byteVal << 24));
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void setOneMFVecFieldValue(MLuint32 *dstVal, const float data[4])
{
  *dstVal = ((static_cast<MLuint32>(data[3])      ) |
             (static_cast<MLuint32>(data[2]) <<  8) |
             (static_cast<MLuint32>(data[1]) << 16) |
             (static_cast<MLuint32>(data[0]) << 24)  );
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void setOneMFVecFieldValue(MLuint32 *dstVal, const unsigned int *rgbaVal)
{
  *dstVal = ( (( (*rgbaVal) >> 24) & 0x000000ff) |
              (( (*rgbaVal) >>  8) & 0x0000ff00) |
              (( (*rgbaVal) <<  8) & 0x00ff0000) |
              (( (*rgbaVal) << 24) & 0xff000000) );
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
#if defined(_MSC_VER) && !defined(_WIN64) && ((_MSC_VER == 1600) || (_MSC_VER == 1900))
#define SpecializedPCLPointXYZ EIGEN_WORKAROUND_MSVC_STL_SUPPORT(pcl::PointXYZ)
#else
#define SpecializedPCLPointXYZ pcl::PointXYZ
#endif
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename DST_VAL_TYPE> 
inline void setOneMFVecFieldValueFromPoint(DST_VAL_TYPE *dstVal, const SpecializedPCLPointXYZ &pnt, bool useData_n)
{
  const float zeroData_n[4] = {0.f, 0.f, 0.f, 0.f};
  setOneMFVecFieldValue(dstVal, useData_n ? zeroData_n : pnt.data);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_TYPE, typename DST_VAL_TYPE> 
inline void setOneMFVecFieldValueFromPoint(DST_VAL_TYPE *dstVal, const POINT_TYPE &pnt, bool useData_n)
{
  setOneMFVecFieldValue(dstVal, useData_n ? pnt.data_n : pnt.data);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename DST_VAL_TYPE> 
inline void setTwoMFVecFieldValuesFromPoint(DST_VAL_TYPE *dstVal, const SpecializedPCLPointXYZ &pnt)
{
  setOneMFVecFieldValue(dstVal  , pnt.data);
  setOneMFVecFieldValue(dstVal+1, pnt.data);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_TYPE, typename DST_VAL_TYPE> 
inline void setTwoMFVecFieldValuesFromPoint(DST_VAL_TYPE *dstVal, const POINT_TYPE &pnt)
{
  setOneMFVecFieldValue(dstVal  , pnt.data  );
  setOneMFVecFieldValue(dstVal+1, pnt.data_n);
  dstVal[1] += dstVal[0];
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
enum PCLMFVecStorageTypes {
  StoreIntensitReplacementAsRGB = 0,
  StoreIntensitReplacementAsRGBA,   
  UseOnlyRGBA,                      
  CopyFromData,                     
  CopyFromData_n,                   
  CopyDataAndData_n                 
};
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
MLPCL_InventorAdapters_EXPORT void getRainbowRGB_From0_to_1(float intensity, float retRGB[3]);
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_CLOUD_TYPE, typename SM_MF_VEC_FIELD_TYPE>
inline size_t setMFVecFromPointCloud(const POINT_CLOUD_TYPE &pointCloud, 
                                     SM_MF_VEC_FIELD_TYPE &mfvecField,
                                     PCLMFVecStorageTypes storageType, 
                                     bool duplicateColors=false,
                                     bool mapToRainbow=false,
                                     bool useCurvatureForColor=false,
                                     bool scaleFromInputRange=false,
                                     float inRangeMin=0.f,
                                     float inRangeMax=1.f)
{
  // Ensure default scaling if scaleFromInputRange is false.
  if (!scaleFromInputRange){
    inRangeMin=0.f;
    inRangeMax=1.f;
  }
  // Precalculate time critical calculations by inverting divisor safely.
  const float scaleWidth = fabs(inRangeMax-inRangeMin) < 10e-6f ? 10e-6f : inRangeMax-inRangeMin;
  const float scaleWidthInv = 1.f/scaleWidth;
  
  bool isSet = false;
  const size_t numPoints = pointCloud.points.size();
  if ((numPoints * (CopyDataAndData_n == storageType ? 2 : 1)) <= ML_NAMESPACE::PCLSupportTools::getSigned32BitMaximumLimit()){
    const unsigned int numPointsAsUInt = mlrange_cast<unsigned int>(numPoints);
    if ((CopyFromData   == storageType) ||
        (CopyFromData_n == storageType)){
      mfvecField.setNum(mlrange_cast<int>(numPoints));
 
      // Enable fast direct pointer array editing on the inventor field data.
      auto dataPtr = mfvecField.startEditing();
      const bool copyFromData_n = (CopyFromData_n == storageType);
      for (unsigned int c=0; c < numPointsAsUInt; ++c){
        // Note: No scaling, because it is not very useful on RGB[A] data, it is needed only for incoming scalar colour scaling.
        setOneMFVecFieldValueFromPoint(dataPtr+c, pointCloud.points[c], copyFromData_n);
      }
      mfvecField.finishEditing();
      isSet = true;
    }
    else if (UseOnlyRGBA  == storageType){
      // Enable fast direct pointer array editing on the inventor field data.
      mfvecField.setNum(mlrange_cast<int>(numPoints * (duplicateColors ? 2u:1u)));
      auto dataPtr = mfvecField.startEditing();
      // Use direct copying of .rgba to uint32 inventor data values.
      for (unsigned int c=0; c < numPointsAsUInt; ++c){
        const unsigned int rgbaVal = ML_NAMESPACE::PCLSupportTools::getPointRGBA(pointCloud.points[c]);
        // Note: No scaling, because it is not very useful on RGB[A] data, it is needed only for incoming scalar colour scaling.
        if (duplicateColors){
          setOneMFVecFieldValue(dataPtr+c*2  , &rgbaVal);
          setOneMFVecFieldValue(dataPtr+c*2+1, &rgbaVal);
        }
        else{
          setOneMFVecFieldValue(dataPtr+c    , &rgbaVal);
        }
      }
      mfvecField.finishEditing();
      isSet = true;
    }
    else if ((StoreIntensitReplacementAsRGB  == storageType) ||
             (StoreIntensitReplacementAsRGBA == storageType)){
      // Enable fast direct pointer array editing on the inventor field data.
      mfvecField.setNum(mlrange_cast<int>(numPoints * (duplicateColors ? 2u:1u)));
      auto dataPtr = mfvecField.startEditing();
      // Use assignment of any value usable as intensity to RGBA voxels as grey values
      // where the intensity value is used as an rgb triple or - if requested - as rgba quadrupel.
      float data[4]={0,0,0,255.9f};
      const bool isRGBAAssignment = (StoreIntensitReplacementAsRGBA == storageType);
      
      // An often used mode is that intensity and transparency are the same value; then we can 
      // assign the value more effectively.
      const bool allChannelsAreTheSame = isRGBAAssignment && !useCurvatureForColor && !mapToRainbow;
      
      float curvatureVal = 0.f;
      for (unsigned int c=0; c < numPointsAsUInt; ++c){
        float dataVal = ML_NAMESPACE::PCLSupportTools::getIntensityReplacement(pointCloud.points[c]);
        if      (dataVal < inRangeMin){ dataVal = inRangeMin; }
        else if (dataVal > inRangeMax){ dataVal = inRangeMax; }
        if (scaleFromInputRange){
          dataVal = (dataVal-inRangeMin)*scaleWidthInv;
        }
        // If dataVal is the same four RGBA values then use optimized version below.
        if (!allChannelsAreTheSame){
          // Channels differ, the four RGBA channels come from different sources.
          float intensityVal = dataVal; // -> color RGB
          float alphaVal     = dataVal; // -> transparency
          
          // What is used as intensity (i.e. for RGB channels)?
          if (useCurvatureForColor){
            // Use curvature instead 
            curvatureVal = ML_NAMESPACE::PCLSupportTools::getCurvature(pointCloud.points[c]);
            if      (curvatureVal < inRangeMin){ curvatureVal = inRangeMin; }
            else if (curvatureVal > inRangeMax){ curvatureVal = inRangeMax; }
            if (scaleFromInputRange){
              curvatureVal = (curvatureVal-inRangeMin)*scaleWidthInv;
            }
            intensityVal = curvatureVal;
          }
 
          // Translate intensity to a rainbow color table (especially interesting for curvature values).
          if (mapToRainbow){
            float retRGB[3]={0.f, 0.f, 0.f};
            getRainbowRGB_From0_to_1(intensityVal, retRGB);
            data[0] = retRGB[0]*255.999f;
            data[1] = retRGB[1]*255.999f;
            data[2] = retRGB[2]*255.999f;
          }
          else{
            // Use the same value for R, G, and B.
            data[0] = data[1] = data[2] = intensityVal*255.999f;
          }
 
          if (isRGBAAssignment){
            // Opacity is related to dataVal.
            data[3] = alphaVal*255.999f;
          }
          else{
            // Opacity shall always be dense.
            data[3] = 255.999f;
          }
          if (duplicateColors){
            setOneMFVecFieldValue(dataPtr+c*2  , data);
            setOneMFVecFieldValue(dataPtr+c*2+1, data);
          }
          else{
            setOneMFVecFieldValue(dataPtr+c, data);
          }
        }
        else{
          dataVal*=255.999f;
          // Use optimized version with a single float for all components.
          if (duplicateColors){
            setOneMFVecFieldValue(dataPtr+c*2  , dataVal);
            setOneMFVecFieldValue(dataPtr+c*2+1, dataVal);
          }
          else{
            setOneMFVecFieldValue(dataPtr+c,     dataVal);
          }
        }
      }
      mfvecField.finishEditing();
      isSet = true;
    }
    else if (CopyDataAndData_n == storageType){
      // Enable fast direct pointer array editing on the inventor field data.
      mfvecField.setNum(mlrange_cast<int>(numPoints*2u));
      auto dataPtr = mfvecField.startEditing();
      for (unsigned int c=0; c < numPointsAsUInt; ++c){
        // Note: No scaling, because it is not very useful on vector data, it is needed only for incoming scalar colour scaling.
        setTwoMFVecFieldValuesFromPoint(dataPtr+(c*2u), pointCloud.points[c]);
      }
      mfvecField.finishEditing();
      isSet = true;
    }
    else{
      ML_PRINT_ERROR("mlPCLInventorTools.h: setMFVecFromPointCloud", ML_BAD_PARAMETER, 
                     "Bad enumerator value passed. Not setting any MField values.");
    }
  }
  else{
    ML_PRINT_ERROR("mlPCLInventorTools.h: setMFVecFromPointCloud", ML_BAD_PARAMETER, 
                   "Inventor cannot handle field sets with more than ML_INT32_MAX points which, "
                   "however, are required for the current input data conversion. Not setting the MField values.");
  }
  if (!isSet){
    mfvecField.deleteValues(0);
  }
  return isSet ? numPoints : 0u;
}
 
 
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
MLPCL_InventorAdapters_EXPORT void setVertices(const std::vector<pcl::Vertices> &verticesVector, 
                                               SoMFInt32                        &mfIntField,
                                               bool                             terminateWithMinusOne);
 
};
 
ML_END_NAMESPACE