milxMath.h

/*=========================================================================
  The Software is copyright (c) Commonwealth Scientific and Industrial Research Organisation (CSIRO)
  ABN 41 687 119 230.
  All rights reserved.

  Licensed under the CSIRO BSD 3-Clause License
  You may not use this file except in compliance with the License.
  You may obtain a copy of the License in the file LICENSE.md or at

  https://stash.csiro.au/projects/SMILI/repos/smili/browse/license.txt

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
=========================================================================*/
#ifndef __MILXMATH_H
#define __MILXMATH_H

#ifndef VTK_ONLY
  //VNL
  #include <vnl/vnl_error.h>
  #include <vnl/vnl_matrix.h>
  #include <vnl/vnl_vector_fixed.h>
  #include <vnl/algo/vnl_matrix_inverse.h>
#endif
#ifndef ITK_ONLY
  //VTK
  #include <vtkMath.h>
  #include <vtkPoints.h>
#endif
//SMILI
#include <milxGlobal.h>

namespace milx
{

template<class Type = double>
class SMILI_EXPORT Math
{
public:
#ifndef VTK_ONLY

  static Type Centroid(const vnl_vector<Type> &data);
  static vnl_vector<Type> Centroid(const vnl_matrix<Type> &data);
  static Type CentroidSize(const vnl_matrix<Type> &data, const vnl_vector<Type> &centroid, bool norm = false);
#ifndef ITK_ONLY
  static vnl_vector<Type> Centroid(vtkPoints *points);
  static Type CentroidSize(vtkPoints *points, const vnl_vector<Type> &centroid, bool norm = false);
#endif
#endif
#ifdef VTK_ONLY
  static Type* Centroid(vtkPoints *points);
  static Type CentroidSize(vtkPoints *points, const Type* centroid, bool norm = false);
#endif

#ifndef VTK_ONLY
  static vnl_matrix<Type> CovarianceMatrix(vnl_vector<Type> sourceVector);
  static vnl_matrix<Type> CovarianceMatrix(vnl_vector<Type> sourceVector, vnl_vector<Type> targetVector);
  static vnl_matrix<Type> CovarianceMatrix(const vnl_matrix<Type> &data, const vnl_vector<Type> &centroid);
  static vnl_matrix<Type> CovarianceMatrix(const vnl_matrix<Type> &data);
#ifndef ITK_ONLY
  static vnl_matrix<Type> CovarianceMatrix(vtkPoints *points, const vnl_vector<Type> &centroid);
#endif
#endif

#ifndef VTK_ONLY
  static Type MahalanobisDistance(const vnl_vector<Type> &source, const vnl_vector<Type> &target);
  static Type MahalanobisDistance(const vnl_vector<Type> &target, const vnl_vector<Type> &mean, const vnl_matrix<Type> &invCovMatrix);
  static Type MahalanobisDistance(const vnl_vector<Type> &target, const vnl_vector<Type> &mean, const vnl_matrix<Type> &covMatrix, vnl_matrix<Type> &invCovMatrix);
#endif

  static Type MeanSquaredError(vtkPoints *sourcePoints, vtkPoints *targetPoints);

protected:

private:
  Math() {};
  ~Math() {};

};

#ifndef VTK_ONLY
template<class Type>
Type Math<Type>::Centroid(const vnl_vector<Type> &data)
{
  const vnl_size_t n = data.size();
  Type centroid = 0.0;

  for (vnl_size_t j = 0; j < n; j ++)
    centroid += data(j); 
  centroid /= n; 

  return centroid;
}

template<class Type>
vnl_vector<Type> Math<Type>::Centroid(const vnl_matrix<Type> &data)
{
  const vnl_size_t n = data.rows();
  const vnl_size_t m = data.cols();
  vnl_vector<Type> centroid(m, 0.0);

  for (vnl_size_t j = 0; j < n; j ++)
  {
    centroid += data.get_row(j); 
  }
  centroid /= n; 

  return centroid;
}

template<class Type>
Type Math<Type>::CentroidSize(const vnl_matrix<Type> &data, const vnl_vector<Type> &centroid, bool norm)
{
  const vnl_size_t n = data.rows();
  Type newScale = 0;

  for (int j = 0; j < n; j ++)
  {
    vnl_vector<Type> rowVector( data.get_row(j) );

    newScale += vtkMath::Distance2BetweenPoints(rowVector.data_block(), centroid.data_block());
  }
  
  if(norm)
    newScale /= n;

  return sqrt(newScale); 
}

#ifndef ITK_ONLY
template<class Type>
vnl_vector<Type> Math<Type>::Centroid(vtkPoints *points)
{
  const vtkIdType n = points->GetNumberOfPoints();
  vnl_vector_fixed<Type, 3> centroid(0.0);

  for (vtkIdType j = 0; j < n; j ++)
  {
    vnl_vector_fixed<Type, 3> location( points->GetPoint(j) );

    centroid += location; 
  }
  centroid /= n; 

  return centroid.as_vector();
}

template<class Type>
Type Math<Type>::CentroidSize(vtkPoints *points, const vnl_vector<Type> &centroid, bool norm)
{
  Type newScale = 0;

  for (vtkIdType j = 0; j < points->GetNumberOfPoints(); j ++)
  {
    vnl_vector_fixed<Type, 3> location( points->GetPoint(j) );

    newScale += vtkMath::Distance2BetweenPoints(location.data_block(), centroid.data_block());
  }
  
  if(norm)
    newScale /= points->GetNumberOfPoints();

  return sqrt(newScale); 
}
#endif
#endif

#ifdef VTK_ONLY //Code that does not depend on VNL
template<class Type>
Type* Math<Type>::Centroid(vtkPoints *points)
{
  const vtkIdType n = points->GetNumberOfPoints();
  //~ Type centroid[3] = {0.0, 0.0, 0.0};
  Type *centroid = new Type[3];

  for (vtkIdType j = 0; j < n; j ++)
  {
    Type location[3];
    
    points->GetPoint(j, location);

    centroid[0] += location[0]/n; 
    centroid[1] += location[1]/n; 
    centroid[2] += location[2]/n; 
  }

  return centroid;
}

template<class Type>
Type Math<Type>::CentroidSize(vtkPoints *points, const Type* centroid, bool norm)
{
  Type newScale = 0;

  for (vtkIdType j = 0; j < points->GetNumberOfPoints(); j ++)
  {
    Type location[3];
    
    points->GetPoint(j, location);

    newScale += vtkMath::Distance2BetweenPoints(location, centroid);
  }
  
  if(norm)
    newScale /= points->GetNumberOfPoints();

  return sqrt(newScale); 
}
#endif

#ifndef VTK_ONLY
template<class Type>
vnl_matrix<Type> Math<Type>::CovarianceMatrix(vnl_vector<Type> sourceVector)
{
  const vnl_size_t n = sourceVector.size();

  sourceVector -= sourceVector.mean();

  vnl_matrix<Type> covarianceMatrix(n, n, 0.0);
  covarianceMatrix += outer_product(sourceVector, sourceVector); 

  return covarianceMatrix;
}

template<class Type>
vnl_matrix<Type> Math<Type>::CovarianceMatrix(vnl_vector<Type> sourceVector, vnl_vector<Type> targetVector)
{
  const vnl_size_t n = sourceVector.size();
  const vnl_size_t m = targetVector.size();

  if (m != n)
  {
    vnl_error_vector_dimension ("Math<Type>::CovarianceMatrix", m, n);
  }

  sourceVector -= sourceVector.mean();
  targetVector -= targetVector.mean();

  vnl_matrix<Type> covarianceMatrix(n, n, 0.0);
  covarianceMatrix += outer_product(sourceVector, targetVector); 

  return covarianceMatrix;
}

template<class Type>
vnl_matrix<Type> Math<Type>::CovarianceMatrix(const vnl_matrix<Type> &data, const vnl_vector<Type> &centroid)
{
  const vnl_size_t n = data.rows();
  const vnl_size_t m = data.cols();
  vnl_size_t norm = 1;

  if (n > 1)
    norm = n-1;

  if (m != centroid.size())
  {
    vnl_error_vector_dimension ("Math<Type>::CovarianceMatrix", m, centroid.size());
  }

  vnl_matrix<Type> covarianceMatrix(m, m, 0.0);
  for (vnl_size_t j = 0; j < n; j ++)
  {
    vnl_vector<Type> rowVector(data.get_row(j));

    rowVector -= centroid; 

    covarianceMatrix += outer_product(rowVector, rowVector) / norm; 
  }

  return covarianceMatrix;
}

template<class Type>
vnl_matrix<Type> Math<Type>::CovarianceMatrix(const vnl_matrix<Type> &data)
{
  const vnl_size_t n = data.rows();
  const vnl_size_t m = data.cols();
  vnl_size_t norm = 1;

  if (n > 1)
    norm = n-1;

  vnl_matrix<Type> covarianceMatrix(m, m, 0.0);
  for (vnl_size_t j = 0; j < n; j ++)
  {
    vnl_vector<Type> rowVector(data.get_row(j));

    covarianceMatrix += outer_product(rowVector, rowVector) / norm; 
  }

  return covarianceMatrix;
}

#ifndef ITK_ONLY
template<class Type>
vnl_matrix<Type> Math<Type>::CovarianceMatrix(vtkPoints *points, const vnl_vector<Type> &centroid)
{
  const vnl_size_t n = points->GetNumberOfPoints();
  vnl_size_t norm = 1;

  if (n > 1)
    norm = n-1;

  if (centroid.size() != 3)
  {
    vnl_error_vector_dimension ("Math<Type>::CovarianceMatrix", 3, centroid.size());
  }

  vnl_matrix<Type> covarianceMatrix(3, 3, 0.0);
  for (vnl_size_t j = 0; j < n; j ++)
  {
    vnl_vector_fixed<Type, 3> location( points->GetPoint(j) );

    location -= centroid; 

    covarianceMatrix += outer_product(location.as_vector(), location.as_vector()) / norm; 
  }

  return covarianceMatrix;
}
#endif
#endif

#ifndef VTK_ONLY
template<class Type>
Type Math<Type>::MahalanobisDistance(const vnl_vector<Type> &source, const vnl_vector<Type> &target)
{
  const vnl_size_t n = source.size();
  const vnl_size_t m = target.size();

  if (n != m)
  {
    //Throw exception
    vnl_error_vector_dimension ("Math<Type>::MahalanobisDistance", n, m);
  }

  /*vnl_matrix<Type> dataMatrix(2, n, 0.0);
  dataMatrix.set_row(0, target);
  dataMatrix.set_row(1, source);

  //    cout << "Compute Centroid" << endl;
  const vnl_vector<Type> dataCentroid = milx::Math<Type>::Centroid(dataMatrix);
    cout << "Centroid: " << dataCentroid << endl;
  //    cout << "Compute Covariance Matrix" << endl;
  vnl_matrix<Type> dataCovariance = milx::Math<Type>::CovarianceMatrix(dataMatrix, dataCentroid);
  cout << "Covariance: " << dataCovariance.rows() << "x" << dataCovariance.cols() << endl;
    cout << "Covariance: \n" << dataCovariance << endl;*/

  vnl_matrix<Type> dataMatrix(n, 2, 0.0);
  dataMatrix.set_column(0, target);
  dataMatrix.set_column(1, source);

  const vnl_vector<Type> dataCentroid = Math<Type>::Centroid(dataMatrix);
  //cout << "Centroid: " << dataCentroid << endl;
  vnl_matrix<Type> dataCovariance = Math<Type>::CovarianceMatrix(dataMatrix, dataCentroid);
  //cout << "Cov: " << dataCovariance.rows() << "x" << dataCovariance.cols() << endl;
  //cout << "Covariance: \n" << dataCovariance << endl;

  /*vnl_matrix<Type> dataMatrix(m, 1, 0.0);
  dataMatrix.set_column(0, target);

  //    const Type centroid = milx::Math<Type>::Centroid(target);
  //    const vnl_vector<Type> dataCentroid(m, centroid);
    const vnl_vector<Type> dataCentroid = milx::Math<Type>::Centroid(dataMatrix);
    cout << "Centroid: " << dataCentroid << endl;
  vnl_matrix<Type> dataCovariance = milx::Math<Type>::CovarianceMatrix(dataMatrix, dataCentroid);
  cout << "Cov: " << dataCovariance.rows() << "x" << dataCovariance.cols() << endl;
    cout << "Covariance: \n" << dataCovariance << endl;*/

  /*vnl_matrix<Type> dataMatrix(1, m, 0.0);
  dataMatrix.set_row(0, target);

  //    const vnl_vector<Type> dataCentroid = milx::Math<Type>::Centroid(dataMatrix);
  //    const Type centroid = milx::Math<Type>::Centroid(target);
  //    const vnl_vector<Type> dataCentroid(m, centroid);
  const vnl_vector<Type> dataCentroid(m, 0.0);
  cout << "Centroid: " << source << endl;
  vnl_matrix<Type> dataCovariance = milx::Math<Type>::CovarianceMatrix(dataMatrix, dataCentroid);
  cout << "Cov: " << dataCovariance.rows() << "x" << dataCovariance.cols() << endl;
  dataCovariance.set_identity();
  cout << "Covariance: \n" << dataCovariance << endl;*/

//    vnl_matrix<Type> dataCovariance = milx::Math<Type>::CovarianceMatrix(source, target);

//    vnl_matrix<Type> dataCovariance = milx::Math<Type>::CovarianceMatrix(target);

//      vnl_matrix<Type> dataDifference(n, 1, 0.0);
  vnl_matrix<Type> dataDifference = dataMatrix;
//    const vnl_vector<Type> sourceZeroMean = source - source.mean();
//    const vnl_vector<Type> targetZeroMean = target - target.mean();
//    dataDifference.set_column(0, targetZeroMean - sourceZeroMean);
//    dataDifference.set_column(0, target - source);

  //cout << "Compute Inverse Covariance Matrix" << endl;
  vnl_matrix<Type> dataInvCovariance = vnl_matrix_inverse<Type>(dataCovariance);
  //cout << "Inv Covariance: \n" << dataInvCovariance << endl;

//      const vnl_matrix<Type> distance = dataDifference.transpose()*dataInvCovariance*dataDifference;
  const vnl_matrix<Type> distance = dataDifference*dataInvCovariance*dataDifference.transpose();

  //cout << "Distance Size: " << distance.rows() << "x" << distance.cols() << endl;
  //cout << "Distance: " << distance << endl;
  return 1.0/distance(0,0); //cause of covariance, maximum correlation is returned so invert
}

template<class Type>
Type Math<Type>::MahalanobisDistance(const vnl_vector<Type> &target, const vnl_vector<Type> &mean, const vnl_matrix<Type> &invCovMatrix)
{
  const vnl_size_t n = target.size();
  const vnl_size_t m = mean.size();

  if (n != m)
  {
    //Throw exception
    vnl_error_vector_dimension ("Math<Type>::MahalanobisDistance", n, m);
  }

  vnl_matrix<Type> dataDifference(n, 1, 0.0);
  dataDifference.set_column(0, target - mean);

  const vnl_matrix<Type> distance = dataDifference.transpose()*invCovMatrix*dataDifference;

  return distance(0,0);
}

template<class Type>
Type Math<Type>::MahalanobisDistance(const vnl_vector<Type> &target, const vnl_vector<Type> &mean, const vnl_matrix<Type> &covMatrix, vnl_matrix<Type> &invCovMatrix)
{
  vnl_matrix<Type> dataInvCovariance = vnl_matrix_inverse<Type>(covMatrix);

  invCovMatrix = dataInvCovariance;

  return MahalanobisDistance(target, mean, invCovMatrix);
}

#ifndef ITK_ONLY
template<class Type>
Type Math<Type>::MeanSquaredError(vtkPoints *sourcePoints, vtkPoints *targetPoints)
{
  const vnl_size_t numberOfPoints = sourcePoints->GetNumberOfPoints();
  Type mse = 0.0;

  for (vnl_size_t v = 0; v < numberOfPoints; v ++) 
  {
    coordinate p( sourcePoints->GetPoint(v) );
    coordinate p2( targetPoints->GetPoint(v) );

    mse += vtkMath::Distance2BetweenPoints(p.data_block(), p2.data_block());
  }
  mse /= numberOfPoints;

  return mse;
}
#endif
#endif

#ifdef VTK_ONLY //Code that does not depend on VNL
template<class Type>
Type Math<Type>::MeanSquaredError(vtkPoints *sourcePoints, vtkPoints *targetPoints)
{
  const size_t numberOfPoints = sourcePoints->GetNumberOfPoints();
  Type mse = 0.0;

  for (size_t v = 0; v < numberOfPoints; v ++) 
  {
    Type p[3], p2[3];
    sourcePoints->GetPoint(v, p);
    targetPoints->GetPoint(v, p2);

    mse += vtkMath::Distance2BetweenPoints(p, p2)/numberOfPoints;
  }

  return mse;
}
#endif

} //end namespace milx

#endif //__MILXMATH_H