mlPCLRegistration.h

Go to the documentation of this file.

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

//----------------------------------------------------------------------------------
#pragma once
 
#include "MLPCLRegistrationSystem.h"
#include <mlPCLModule.h>
 
ML_START_NAMESPACE
 
//----------------------------------------------------------------------------------
//----------------------------------------------------------------------------------
class MLPCL_Registration_EXPORT PCLRegistration : public PCLModule
{
public:
  PCLRegistration(int numInputs=2, int numOutputs=1);

  IntField     & getMaximumIterationsFld             () const { return *_maximumIterationsFld;             }
  DoubleField  & getMaxCorrespondenceDistanceFld     () const { return *_maxCorrespondenceDistanceFld;     }
  DoubleField  & getTransformationEpsilonFld         () const { return *_transformationEpsilonFld;         }
  DoubleField  & getEuclideanFitnessEpsilonFld       () const { return *_euclideanFitnessEpsilonFld;       }
  
  Matrix4Field & getFinalTransformationFld           () const { return *_finalTransformationFld;           }
  Matrix4Field & getLastIncrementalTransformationFld () const { return *_lastIncrementalTransformationFld; }
  DoubleField  & getFitnessScoreFld                  () const { return *_fitnessScoreFld;                  }
  BoolField    & getHasConvergedFld                  () const { return *_hasConvergedFld;                  }
  
protected:
  void activateAttachments() override;

  void handleNotification (Field* field) override;

  void _addPCLRelatedFields();

  static inline Matrix4 _pclToMLType(Eigen::Matrix<float, 4, 4> inputType)
  {
    const double floatDigitsMultiplier = pow(10., FLT_DIG+1);
    Matrix4 retMatrix;
    double intPart=0;
    for (int c=0; c < 4; ++c){
      for (int r=0; r < 4; ++r){
        // Multiply with 10 ^ available floating point digits,
        // take integer part and divide it to get original number 
        // without stuff after floating point digits in double values.
        modf(inputType.col(c)[r]*floatDigitsMultiplier, &intPart);
        retMatrix[static_cast<size_t>(c)][static_cast<size_t>(r)] = intPart/floatDigitsMultiplier;
      }
    }
    return retMatrix;
  }
  
  template <typename PCL_FILTER>
  inline void _setFilterParameters(PCL_FILTER &regObject) const
  {
    regObject.setMaximumIterations        (mlrange_cast<int>(getMaximumIterationsFld().getIntValue()));
    regObject.setMaxCorrespondenceDistance(getMaxCorrespondenceDistanceFld().getDoubleValue());
    regObject.setTransformationEpsilon    (getTransformationEpsilonFld().getDoubleValue());
    regObject.setEuclideanFitnessEpsilon  (getEuclideanFitnessEpsilonFld().getDoubleValue()); 
  }

  template <typename PCL_FILTER>
  inline void _retrieveFilterResultParameters(PCL_FILTER &regObject)
  {
    getFitnessScoreFld().setDoubleValue(regObject.getFitnessScore());
    getFinalTransformationFld().setMatrixValue(_pclToMLType(regObject.getFinalTransformation()));
    getLastIncrementalTransformationFld().setMatrixValue(_pclToMLType(regObject.getLastIncrementalTransformation()));
    getHasConvergedFld().setBoolValue(regObject.hasConverged());
  }
  
private:
  
  void _clampValues();
 
  // ---- Input fields:
  IntField     *_maximumIterationsFld;
  
  DoubleField  *_maxCorrespondenceDistanceFld;
  
  DoubleField  *_transformationEpsilonFld;
  
  DoubleField  *_euclideanFitnessEpsilonFld;
  
  // ---- Output fields:
  Matrix4Field *_finalTransformationFld;
  
  Matrix4Field *_lastIncrementalTransformationFld;

  DoubleField  *_fitnessScoreFld;

  BoolField    *_hasConvergedFld;

  virtual void _filter() = 0;
  
  ML_ABSTRACT_MODULE_CLASS_HEADER(PCLRegistration);
};
 
ML_END_NAMESPACE