mlPCLStatisticalClusterInfo.h

Go to the documentation of this file.

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

//----------------------------------------------------------------------------------
#pragma once
 
#include <MLPCLClusterStatisticsSystem.h>
#include <mlPCLSupportTools.h>
#include <mlPCLSupportStatistics.h>
#include <mlLine.h>
#include <mlPlane.h>
#include <PCLThirdPartyWarningsDisable.h>
#include <pcl/common/pca.h>
#include <pcl/common/io.h>
#include <pcl/common/common.h>
#include <PCLThirdPartyWarningsRestore.h>
 
ML_START_NAMESPACE
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
struct StatisticalClusterInfo {
  inline StatisticalClusterInfo(){ 
    clear(); 
  }
  
  inline void clear()
  {
    numPoints = 0;
    
    indices.clear();
    
    magnitudeSum = 0.;
    magnitudeSumAbsolute = 0.;
    magnitudeAverage = 0.;
    magnitudeAverageAbsolute = 0.;
    magnitudeMedian = 0.;
    
    phaseSum = 0.;
    phaseSumAbsolute = 0.;
    phaseAverage = 0.;
    phaseAverageAbsolute = 0.;
    phaseMedian = 0.;
    
    centreOfGravity.assign(0.,0.,0.);
    eigenValues.assign(0.,0.,0.);
    eigenVectors[0].assign(0.,0.,0.);
    eigenVectors[1].assign(0.,0.,0.);
    eigenVectors[2].assign(0.,0.,0.);
    orientedExtentsMin.assign(0.,0.,0.);
    orientedExtentsMax.assign(0.,0.,0.);
    
    planeIntersection.assign(0.,0.,0.);
    planeIntersectionOk = false;
  }
  
  inline void insert(int index, double magnitude, double phase)
  {
    numPoints++;
    
    indices.push_back(index);
    
    magnitudeSum += magnitude;
    magnitudeSumAbsolute += fabs(magnitude);
    
    phaseSum += phase;
    phaseSumAbsolute += fabs(phase);
  }
  
  inline void finalize(const MLPointCloudXYZINormal &inputPointCloud,
                       const Vector3 &planeNormal, 
                       const Vector3 &planePoint,
                       const std::string &magnitudeFloatMemberName,
                       const std::string &phaseFloatMemberName)
  {
    if (numPoints > 0){
      const double numPointsDbl = static_cast<double>(numPoints);
      
      // Compute magnitude results.
      magnitudeAverage = magnitudeSum / numPointsDbl;
      magnitudeAverageAbsolute = magnitudeSumAbsolute / numPointsDbl;
      
      // Compute phase results.
      phaseAverage = phaseSum / numPointsDbl;
      phaseAverageAbsolute = phaseSumAbsolute / numPointsDbl;
      
      // Compute Eigenvectors, -values, and centre of gravity.
      MLPointCloudXYZINormal clusterCloud;
      pcl::copyPointCloud(inputPointCloud, indices, clusterCloud);
      
      // Calculate the two median values.
      PCLSupportTools::PointCloudStatistics stats;
      stats = PCLSupportTools::getStatistics(clusterCloud, magnitudeFloatMemberName, true);
      magnitudeMedian = stats.median;
      stats = PCLSupportTools::getStatistics(clusterCloud, phaseFloatMemberName, true);
      phaseMedian = stats.median;
      
      // Finally calculate the principle components, eigenvalues etc.
      if (clusterCloud.points.size() >=3 ){
        // Compute principle component analysis only if there are more 
        // than 3 points, because otherwise crashes may appear.
        pcl::PCA<MLPointCloudXYZINormal::PointType> PCAFilter;
        PCAFilter.setInputCloud(PCLMakeLocalNonDeletingSharedConstPtr(clusterCloud));
        const Eigen::Vector4f meanTmp         = PCAFilter.getMean();
        const Eigen::Vector3f eigenValuesTmp  = PCAFilter.getEigenValues();
        const Eigen::Vector3f eigenVector0Tmp = PCAFilter.getEigenVectors().col(0);
        const Eigen::Vector3f eigenVector1Tmp = PCAFilter.getEigenVectors().col(1);
        const Eigen::Vector3f eigenVector2Tmp = PCAFilter.getEigenVectors().col(2);
 
        centreOfGravity.assign( meanTmp[0], meanTmp[1], meanTmp[2] /*, meanTmp[3]*/ );
        eigenValues.assign( eigenValuesTmp[0], eigenValuesTmp[1], eigenValuesTmp[2] );
        eigenVectors[0].assign( eigenVector0Tmp[0], eigenVector0Tmp[1], eigenVector0Tmp[2] );
        eigenVectors[1].assign( eigenVector1Tmp[0], eigenVector1Tmp[1], eigenVector1Tmp[2] );
        eigenVectors[2].assign( eigenVector2Tmp[0], eigenVector2Tmp[1], eigenVector2Tmp[2] );
        
        // Create a point cloud projected to x/y/z-axis.
        MLPointCloudXYZINormal tmpCloud;
        PCAFilter.project(clusterCloud, tmpCloud);
        pcl::PointXYZINormal minPoint, maxPoint;
        pcl::getMinMax3D(tmpCloud, minPoint, maxPoint);
        
        // Determine extent of point cloud along the three eigen vectors.
        orientedExtentsMin[0] = minPoint.x;
        orientedExtentsMin[1] = minPoint.y;
        orientedExtentsMin[2] = minPoint.z;
        orientedExtentsMax[0] = maxPoint.x;
        orientedExtentsMax[1] = maxPoint.y;
        orientedExtentsMax[2] = maxPoint.z;
        
        // Cut line of (centreOfGravity,centreOfGravity+eigenVectors[0]) with user plane;
        // eigenVectors[0] is the one with largest eigenValue.
        Plane plane(planeNormal, planePoint);
        Line line(centreOfGravity, centreOfGravity+eigenVectors[0]);
        planeIntersectionOk = plane.intersect(line, planeIntersection);
      }
    }
  }
  
  MLint numPoints;
  
  std::vector<int> indices;
  
  // Magnitude:
  double magnitudeSum;
  
  double magnitudeSumAbsolute;
  
  double magnitudeAverage;
  
  double magnitudeAverageAbsolute;
  
  double magnitudeMedian;
 
  
  // Phase:
  double phaseSum;
  
  double phaseSumAbsolute;
  
  double phaseAverage;
  
  double phaseAverageAbsolute;
  
  double phaseMedian;
  
  
  Vector3 centreOfGravity;

  Vector3 eigenValues;

  Vector3 eigenVectors[3];

  Vector3 orientedExtentsMin;
  
  Vector3 orientedExtentsMax;
  
  Vector3 planeIntersection;

  bool planeIntersectionOk;
};
 
ML_END_NAMESPACE