mlPCLMLTools.h

Go to the documentation of this file.

// Copyright (c) Fraunhofer MEVIS, Germany. All rights reserved.
// **InsertLicense** code author="Wolf Spindler"
//----------------------------------------------------------------------------------

//----------------------------------------------------------------------------------
#pragma once
 
#include "MLPCLMLAdaptersSystem.h"
#include <mlPCLTypes.h>
#include <mlPCLSupportTools.h>
#include <mlModuleIncludes.h>
#include <mlReleaseToolsString.h>
 
#include <mlPointList.h>
#include <mlVectorList.h>
#include <mlXMarkerList.h>
 
ML_START_NAMESPACE
 
class SubImage;
class PagedImage;

namespace PCLMLTools {
 
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
MLPCL_MLAdapters_EXPORT Vector3f getVector3fFromVoxVal(MLDataType        dt, 
                                                       const MLTypeData* voxVal);
 
//----------------------------------------------------------------------------------
// Adapt this if a new PCL point type is integrated in the MeVisLab binding.
//----------------------------------------------------------------------------------
inline void setVectorReplacement(pcl::PointXYZ & /*p*/, const Vector3f & /*vf*/)
{
  // There is no member which could be used for the vector, do not assign anything.
}
 
// Handle all other point types generically.
template <typename PCL_POINT_TYPE>
inline void setVectorReplacement(PCL_POINT_TYPE &p, const Vector3f &vf)
{ 
  p.normal_x = vf[0];
  p.normal_y = vf[1];
  p.normal_z = vf[2];
}

 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
struct MLPCL_MLAdapters_EXPORT ImageToPointCloudConversionParameters 
{
  ImageToPointCloudConversionParameters(): 
    thresholdMin(0.), thresholdMax(0.), 
    lowerValue(0.), upperValue(0.), 
    skipVoxelsBelowMin(false),
    skipVoxelsAboveMax(false),
    normalizeInClampedRange(false),
    useVectorLengthAsIntensity(false),
    transformToWorld(false){};
  
  double thresholdMin;
  
  double thresholdMax;
  
  double lowerValue;
  
  double upperValue;
  
  bool   skipVoxelsBelowMin;
  
  bool   skipVoxelsAboveMax;

  bool   normalizeInClampedRange;
  
  bool   useVectorLengthAsIntensity;
  
  bool   transformToWorld;
};
 
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_CLOUD_TYPE>
inline void transformTo(POINT_CLOUD_TYPE &pointCloud,
                        const Matrix4    &voxelToWorldMatrix,
                        bool             toWorld)
{
  typedef typename POINT_CLOUD_TYPE::PointType PointType;
  MedicalImageProperties mapProps;
  mapProps.setVoxelToWorldMatrix(voxelToWorldMatrix);
  const size_t numPoints = pointCloud.points.size();
  Vector3 pnt;
  for (size_t c=0; c < numPoints; ++c){
    PointType &pclPoint = pointCloud.points[c];
    pnt.assign(pclPoint.x, pclPoint.y, pclPoint.z);
    if (toWorld){
      pnt = mapProps.mapVoxelToWorld(pnt);
    }
    else{
      pnt = mapProps.mapWorldToVoxel(pnt);
    }
    pclPoint.x = static_cast<float>(pnt[0]);
    pclPoint.y = static_cast<float>(pnt[1]);
    pclPoint.z = static_cast<float>(pnt[2]);
  }
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_CLOUD_PTR_TYPE>
inline bool appendSubImageVoxelsToPointCloud(const SubImage                              &subImg,
                                             const ImageToPointCloudConversionParameters &convParams,
                                             POINT_CLOUD_PTR_TYPE                        retPointCloudPtr)
{
  bool allAddedRet = true;
  const MLTypeInfos* dataTypeInfos = subImg.getDataTypeInfos();
  if (dataTypeInfos && subImg.getData() && retPointCloudPtr){
 
    const MLDataType dt = subImg.getDataType();
    const bool isVectorType = MLIsScalarType(dt) ? false : true;
    double scale=0;
    if (convParams.normalizeInClampedRange){
      const double range = convParams.thresholdMax - convParams.thresholdMin;
      if (range < 10e-10){ 
        scale = 1.; 
      }
      else{
        scale = 1. / range;
      }
    }
    typedef typename POINT_CLOUD_PTR_TYPE::element_type::PointType  PointType;
    typedef typename POINT_CLOUD_PTR_TYPE::element_type::VectorType PointVectorType;
    PointVectorType tmpAddedPoints;
    tmpAddedPoints.reserve(20000);
    const SubImageBox subImgBox = subImg.getBox();
    ImageVector p = subImgBox.v1;
    PointType valToAppend;
    Vector3f vec3fVal;
    //for (p.u = subImgBox.v1.u; p.u <= subImgBox.v2.u; ++p.u){
      //for (p.t = subImgBox.v1.t; p.t <= subImgBox.v2.t; ++p.t){
        //for (p.c = subImgBox.v1.c; p.c <= subImgBox.v2.c; ++p.c){
           for (p.z = subImgBox.v1.z; p.z <= subImgBox.v2.z; ++p.z){
             for (p.y = subImgBox.v1.y; p.y <= subImgBox.v2.y; ++p.y){
               for (p.x = subImgBox.v1.x; p.x <= subImgBox.v2.x; ++p.x){
                 
                 // Get value from the image as generic voxel MLTypeData and determine
                 // whether the value interpreted as double shall be taken or 
                 // alternatively the length of a vector if we have one. 
                 const MLTypeData* voxVal = reinterpret_cast<MLTypeData*>(subImg.getImagePointer(p));
                 double dblVal = 0;
                 if (isVectorType){
                   vec3fVal = getVector3fFromVoxVal(dt, voxVal);
                 }
                 dblVal = isVectorType && convParams.useVectorLengthAsIntensity ? 
                            vec3fVal.length() : 
                            dataTypeInfos->castToDouble(voxVal);
 
                 // Do clamping and scaling.
                 bool add = true;
                 if (dblVal < convParams.thresholdMin){
                   if (convParams.skipVoxelsBelowMin){
                     add = false;
                   }
                   else{
                     dblVal = convParams.lowerValue;
                   }
                 }
                 if (dblVal > convParams.thresholdMax){
                   if (convParams.skipVoxelsAboveMax){
                     add = false;
                   }
                   else{
                     dblVal = convParams.upperValue;
                   }
                 }
                 
                 // Append point with coordinates and intensity.
                 if (!add){
                   allAddedRet = false;
                 }
                 else{
                   valToAppend.x = static_cast<float>(p.x);
                   valToAppend.y = static_cast<float>(p.y);
                   valToAppend.z = static_cast<float>(p.z);
                   if (convParams.normalizeInClampedRange){
                     if (dblVal < convParams.thresholdMin){ dblVal = convParams.thresholdMin; }
                     if (dblVal > convParams.thresholdMax){ dblVal = convParams.thresholdMax; }
                     dblVal = (dblVal-convParams.thresholdMin)*scale;
                   }
                   valToAppend.data[3] = 1.0f;
                   PCLSupportTools::setIntensityReplacement(valToAppend, static_cast<float>(dblVal));
                   if (isVectorType){
                     setVectorReplacement(valToAppend, vec3fVal);
                   }
                   tmpAddedPoints.push_back(valToAppend);
                 }
               }
             }
           }
        //}
      //}
    //}
    if (tmpAddedPoints.size() > 0){
      const MLuint64 newSize = static_cast<MLuint64>(retPointCloudPtr->width) + tmpAddedPoints.size();
      if (newSize < PCLSupportTools::getUnsigned32BitMaximumLimit()){
        retPointCloudPtr->width = mlrange_cast<MLuint32>(newSize);
        retPointCloudPtr->height = 1;
        retPointCloudPtr->points.insert(retPointCloudPtr->points.end(), tmpAddedPoints.begin(), tmpAddedPoints.end());
      }
      else{
        ML_PRINT_ERROR("mlPCLMLTools.cpp: appendSubImageVoxelsToPointCloud",
                       ML_BAD_PARAMETER, 
                       "Could not add points to point cloud since this would exceed "
                       "the maximum number UINT32_MAX points in a point cloud.");
      }
    }
  }
  return allAddedRet;
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_CLOUD_PTR_TYPE>
inline MLErrorCode createPointCloudFromPagedImage(PagedImage                                  &inImg,
                                                  const ImageToPointCloudConversionParameters &convParams,
                                                  POINT_CLOUD_PTR_TYPE                        &retPointCloudPtr)
{
  typedef typename POINT_CLOUD_PTR_TYPE::element_type POINT_CLOUD_TYPE;
  MLErrorCode retErr =  ML_NO_MEMORY;
  retPointCloudPtr = POINT_CLOUD_PTR_TYPE(new POINT_CLOUD_TYPE);
  SubImage subImage(inImg.getBoxFromImageExtent(), inImg.getDataType(), nullptr);
  subImage.allocateAsMemoryBlockHandle(ML_RETURN_NULL);
  bool isOrganized = false;
  if (subImage.getData()){
    retErr = inImg.getTile(subImage);
    if (ML_RESULT_OK == retErr){
      isOrganized = appendSubImageVoxelsToPointCloud(subImage, convParams, retPointCloudPtr);
      
      // If all voxels have been added then we can set the height of the point cloud to mark it as organized. 
      if (isOrganized && retPointCloudPtr && 
          (subImage.getNumVoxels() <= static_cast<MLint>(PCLSupportTools::getUnsigned32BitMaximumLimit()))){
        retPointCloudPtr->width  = mlrange_cast<MLuint32>(subImage.getExtent().x);
        retPointCloudPtr->height = mlrange_cast<MLuint32>(subImage.getExtent().y * subImage.getExtent().z);
      }
    }
  }
  if (convParams.transformToWorld){
    transformTo(*retPointCloudPtr, inImg.getVoxelToWorldMatrix(), true);
  }
  return retErr;
}

// Adapt this if a new PCL point type is integrated in the MeVisLab binding.
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_TYPE>
inline void assignPointDataFromXMarker(const XMarker &xMarker, POINT_TYPE &point)
{
  point.data[0] = static_cast<float>(xMarker.pos[0]);
  point.data[1] = static_cast<float>(xMarker.pos[1]);
  point.data[2] = static_cast<float>(xMarker.pos[2]);
  point.data[3] = 1.0f;
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void assignPointDataFromXMarker(const XMarker &xMarker, pcl::PointXYZLNormal &point)
{
  point.data[0]   = static_cast<float>(xMarker.pos[0]);
  point.data[1]   = static_cast<float>(xMarker.pos[1]);
  point.data[2]   = static_cast<float>(xMarker.pos[2]);
  point.data[3]   = 1.0f;
  point.data_n[0] = static_cast<float>(xMarker.vec[0]);
  point.data_n[1] = static_cast<float>(xMarker.vec[1]);
  point.data_n[2] = static_cast<float>(xMarker.vec[2]);
  point.data_n[3] = 0.0f;
  point.label     = static_cast<unsigned int>(xMarker.type);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void assignPointDataFromXMarker(const XMarker &xMarker, pcl::PointXYZRGBNormal &point)
{
  point.data[0]   = static_cast<float>(xMarker.pos[0]);
  point.data[1]   = static_cast<float>(xMarker.pos[1]);
  point.data[2]   = static_cast<float>(xMarker.pos[2]);
  point.data[3]   = 1.0f;
  point.data_n[0] = static_cast<float>(xMarker.vec[0]);
  point.data_n[1] = static_cast<float>(xMarker.vec[1]);
  point.data_n[2] = static_cast<float>(xMarker.vec[2]);
  point.data_n[3] = 0.0f;
  point.rgba      = static_cast<unsigned int>(xMarker.type);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void assignPointDataFromXMarker(const XMarker &xMarker, pcl::PointXYZINormal &point)
{
  point.data[0]   = static_cast<float>(xMarker.pos[0]);
  point.data[1]   = static_cast<float>(xMarker.pos[1]);
  point.data[2]   = static_cast<float>(xMarker.pos[2]);
  point.data[3]   = 1.0f;
  point.data_n[0] = static_cast<float>(xMarker.vec[0]);
  point.data_n[1] = static_cast<float>(xMarker.vec[1]);
  point.data_n[2] = static_cast<float>(xMarker.vec[2]);
  point.data_n[3] = 0.0f;
  point.intensity = static_cast<float>(xMarker.type);
}

 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_CLOUD_PTR_TYPE>
inline void createPointCloudFromBaseList(const Base           *inBaseList,
                                         POINT_CLOUD_PTR_TYPE &retPointCloudPtr)
{
  if (!inBaseList){
    retPointCloudPtr.reset();
  }
  else{
    // Check for list-like base types.
    const PointList            *pl   = mlbase_cast<const PointList*>(inBaseList);
    const VectorList           *vl   = mlbase_cast<const VectorList*>(inBaseList);
    const XMarkerList          *xml  = mlbase_cast<const XMarkerList*>(inBaseList);
    const XMarkerListContainer *xmlc = mlbase_cast<const XMarkerListContainer*>(inBaseList);
  
    if (pl || vl || xml || xmlc){
      // Create a new point cloud if we have an input object, even if it is empty.
      typedef typename POINT_CLOUD_PTR_TYPE::element_type POINT_CLOUD_TYPE;
      retPointCloudPtr = POINT_CLOUD_PTR_TYPE(new POINT_CLOUD_TYPE);
 
      // Get number of points from list.
      const MLssize_t numVals = (pl   ? pl->getNum()                                     :
                                (vl   ? vl->getNum()                                     :
                                (xml  ? mlrange_cast<MLssize_t>(xml->size())             :
                                (xmlc ? mlrange_cast<MLssize_t>(const_cast<XMarkerListContainer*>(xmlc)->getList()->size()) : 0))));
      if (static_cast<MLint>(numVals) > static_cast<MLint>(PCLSupportTools::getUnsigned32BitMaximumLimit())){
        ML_PRINT_ERROR("mlPCLToMLTools.h: createPointCloudFromBaseList",
                       ML_BAD_PARAMETER,
                       "Input point cloud has more than ML_UINT32_MAX entries; the "
                       "PointCloud.width parameter cannot be set to this value any more; "
                       "using ML_UINT32_MAX instead.");
        retPointCloudPtr->width = static_cast<MLuint32>(PCLSupportTools::getUnsigned32BitMaximumLimit());
      }
      else{
        retPointCloudPtr->width = static_cast<MLuint32>(numVals);
      }
      retPointCloudPtr->height = 1;
      retPointCloudPtr->is_dense = true;
      retPointCloudPtr->points.resize(mlrange_cast<size_t>(numVals));
      
      
      if (numVals > 0){
        for (MLssize_t cs=0; cs < numVals; ++cs){
          typename POINT_CLOUD_TYPE::PointType &point = retPointCloudPtr->points[static_cast<size_t>(cs)];
 
          if (pl){
            pl->getValue(cs, point.data[0], point.data[1], point.data[2]);
          }
          else if (vl){
            int vecType=0;
            vl->getPoint(cs, vecType, point.data[0], point.data[1], point.data[2]);
          }
          else if (xml || xmlc){
            const XMarker &xMarker = xml ? (*static_cast<const XMarker*>(xml->getConstItemAt(cs))) :
                                           (*static_cast<const XMarker*>(const_cast<XMarkerListContainer*>(xmlc)->getList()->getConstItemAt(cs)));
            assignPointDataFromXMarker(xMarker, point);
          }
        }
      }
    }
  }
}
 
 

// Adapt this if a new PCL point type is integrated in the MeVisLab binding.
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_TYPE>
inline void assignXMarkerFromPointData(const POINT_TYPE &point, XMarker &xMarker)
{
  xMarker.pos[0] = point.data[0];
  xMarker.pos[1] = point.data[1];
  xMarker.pos[2] = point.data[2];
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void assignXMarkerFromPointData(const pcl::PointXYZLNormal &point, XMarker &xMarker)
{
  xMarker.pos[0] = point.data[0];
  xMarker.pos[1] = point.data[1];
  xMarker.pos[2] = point.data[2];
  xMarker.vec[0] = point.data_n[0];
  xMarker.vec[1] = point.data_n[1];
  xMarker.vec[2] = point.data_n[2];
  xMarker.type   = static_cast<int>(point.label);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void assignXMarkerFromPointData(const pcl::PointXYZRGBNormal &point, XMarker &xMarker)
{
  xMarker.pos[0] = point.data[0];
  xMarker.pos[1] = point.data[1];
  xMarker.pos[2] = point.data[2];
  xMarker.vec[0] = point.data_n[0];
  xMarker.vec[1] = point.data_n[1];
  xMarker.vec[2] = point.data_n[2];
  xMarker.type   = static_cast<int>(point.rgba);
}
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
inline void assignXMarkerFromPointData(const pcl::PointXYZINormal &point, XMarker &xMarker)
{
  xMarker.pos[0] = point.data[0];
  xMarker.pos[1] = point.data[1];
  xMarker.pos[2] = point.data[2];
  xMarker.vec[0] = point.data_n[0];
  xMarker.vec[1] = point.data_n[1];
  xMarker.vec[2] = point.data_n[2];
  xMarker.type   = static_cast<int>(point.intensity);
}

 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
template <typename POINT_CLOUD_TYPE>
inline void convertPointCloudToXMarkerList(POINT_CLOUD_TYPE &inputPointCloud,
                                           XMarkerList      &outputList)
{
  outputList.clear();
  const size_t numInputPoints = inputPointCloud.points.size();
  outputList.resize(numInputPoints);
  for (size_t c=0; c < numInputPoints; ++c){
    XMarker *xMarker = static_cast<XMarker*>(outputList.getItemAt(static_cast<MLssize_t>(c)));
    assignXMarkerFromPointData(inputPointCloud.points[c], *xMarker);
  }
}
 
};
 
ML_END_NAMESPACE