GaussianProcess.cc

Go to the documentation of this file.

//  - * -  LSST - C++   - * -

/*
 * LSST Data Management System
 * Copyright 2008, 2009, 2010 LSST Corporation.
 *
 * This product includes software developed by the
 * LSST Project (http://www.lsst.org/).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the LSST License Statement and
 * the GNU General Public License along with this program.  If not,
 * see  < http://www.lsstcorp.org/LegalNotices/ > .
 */

#include  <iostream>
#include  <cmath>

#include "lsst/afw/math/GaussianProcess.h"

using namespace std;


namespace lsst {
namespace afw {
namespace math {

GaussianProcessTimer::GaussianProcessTimer()
{
    _interpolationCount = 0;
    _iterationTime = 0.0;
    _eigenTime = 0.0;
    _searchTime = 0.0;
    _varianceTime = 0.0;
    _totalTime = 0.0;

}

void GaussianProcessTimer::reset()
{
    _interpolationCount = 0;
    _iterationTime = 0.0;
    _eigenTime = 0.0;
    _searchTime = 0.0;
    _varianceTime = 0.0;
    _totalTime = 0.0;

}

void GaussianProcessTimer::start()
{
    _before = double(lsst::daf::base::DateTime::now().get()*24*60*60);
    _beginning = _before;
}

void GaussianProcessTimer::addToEigen()
{
    double after;
    after = double(lsst::daf::base::DateTime::now().get()*24*60*60);
    _eigenTime += after - _before;
    _before = after;
}

void GaussianProcessTimer::addToVariance()
{
    double after;
    after = double(lsst::daf::base::DateTime::now().get()*24*60*60);
    _varianceTime += after - _before;
    _before = after;
}

void GaussianProcessTimer::addToSearch()
{
    double after;
    after = double(lsst::daf::base::DateTime::now().get()*24*60*60);
    _searchTime += after - _before;
    _before = after;
}

void GaussianProcessTimer::addToIteration()
{
    double after;
    after = double(lsst::daf::base::DateTime::now().get()*24*60*60);
    _iterationTime += after - _before;
    _before = after;
}

void GaussianProcessTimer::addToTotal(int i)
{
    double after;
    after = double(lsst::daf::base::DateTime::now().get()*24*60*60);
    _totalTime += after - _beginning;
    _interpolationCount += i;
}

void GaussianProcessTimer::display(){
    std::cout << "\nSearch time " << _searchTime << "\n";
    std::cout << "Eigen time " << _eigenTime << "\n";
    std::cout << "Variance time " << _varianceTime << "\n";
    std::cout << "Iteration time " << _iterationTime << "\n";
    std::cout << "Total time " << _totalTime << "\n";
    std::cout << "Number of interpolations " << _interpolationCount << "\n";
}

template  < typename T >
void KdTree < T > ::Initialize(ndarray::Array < T,2,2 >  const &dt)
{
    int i;

    _npts = dt.template getSize < 0 > ();
    _dimensions = dt.template getSize < 1 > ();

    //a buffer to use when first building the tree
    _inn = allocate(ndarray::makeVector(_npts));

    _roomStep = 5000;
    _room = _npts;

    _data = allocate(ndarray::makeVector(_room,_dimensions));

    _data.deep() = dt;

    _tree = allocate(ndarray::makeVector(_room,4));

    for (i = 0 ; i < _npts ; i++ ) {
      _inn[i] = i;
    }

    _organize(_inn,_npts, -1, -1);


    i = _testTree();
    if (i == 0) {
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Failed to properly initialize KdTree\n");
    }

}

template < typename T >
void KdTree < T > ::findNeighbors(ndarray::Array < int,1,1 >  neighdex,
                                  ndarray::Array < double,1,1 >  dd,
                                  ndarray::Array < const T,1,1 >  const &v,
                                  int n_nn) const
{

    if(n_nn > _npts){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for more neighbors than kd tree contains\n");
    }

    if(n_nn <= 0){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for zero or a negative number of neighbors\n");
    }

    if(neighdex.getNumElements() != n_nn){
       throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                         "Size of neighdex does not equal n_nn in KdTree.findNeighbors\n");
    }

    if(dd.getNumElements() != n_nn){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Size of dd does not equal n_nn in KdTree.findNeighbors\n");
    }

    int i,start;

    _neighborCandidates = allocate(ndarray::makeVector(n_nn));
    _neighborDistances = allocate(ndarray::makeVector(n_nn));
    _neighborsFound = 0;
    _neighborsWanted = n_nn;

    for (i = 0; i < n_nn; i++ ) _neighborDistances[i] =  -1.0;

    start = _findNode(v);

    _neighborDistances[0] = _distance(v,_data[start]);
    _neighborCandidates[0] = start;
    _neighborsFound = 1;


    for (i = 1; i < 4; i++ ) {
        if(_tree[start][i] >= 0){
            _lookForNeighbors(v,_tree[start][i], start);
        }
    }

    for (i = 0 ; i < n_nn ; i++ ) {
        neighdex[i] = _neighborCandidates[i];
        dd[i] = _neighborDistances[i];
    }
}

template  < typename T >
T KdTree < T > ::getData(int ipt, int idim) const
{
    if(ipt >= _npts || ipt < 0){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for point that does not exist in KdTree\n");
    }

    if(idim >= _dimensions || idim < 0){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "KdTree does not have points of that many dimensions\n");
    }

    return _data[ipt][idim];
}

template  < typename T >
ndarray::Array < T,1,1 >  KdTree < T > ::getData(int ipt) const
{
    if(ipt >= _npts || ipt<0){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for point that does not exist in KdTree\n");
    }

    return _data[ipt];
}

template  < typename T >
void KdTree < T > ::addPoint(ndarray::Array < const T,1,1 >  const &v)
{

    if(v.getNumElements() != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You are trying to add a point of the incorrect dimensionality to KdTree\n");
    }

    int i,j,node,dim,dir;

    node = _findNode(v);
    dim = _tree[node][DIMENSION] + 1;
    if(dim == _dimensions)dim = 0;

    if(_npts == _room){

        ndarray::Array < T,2,2 >  dbuff = allocate(ndarray::makeVector(_npts, _dimensions));
        ndarray::Array < int,2,2 >  tbuff = allocate(ndarray::makeVector(_npts, 4));

        dbuff.deep() = _data;
        tbuff.deep() = _tree;

        _room += _roomStep;

        _tree = allocate(ndarray::makeVector(_room, 4));
        _data = allocate(ndarray::makeVector(_room, _dimensions));


        for (i = 0; i < _npts; i++ ) {
            for (j = 0; j < _dimensions; j++ ) _data[i][j] = dbuff[i][j];
            for (j = 0; j < 4; j++ ) _tree[i][j] = tbuff[i][j];
        }
    }

    _tree[_npts][DIMENSION] = dim;
    _tree[_npts][PARENT] = node;
    i = _tree[node][DIMENSION];

    if(_data[node][i] > v[i]){
        if(_tree[node][LT] >= 0){
            throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
            "Trying to add to KdTree in a node that is already occupied\n");
        }
        _tree[node][LT] = _npts;
        dir=LT;
    }
    else{
        if(_tree[node][GEQ] >= 0){
            throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
            "Trying to add to KdTree in a node that is already occupied\n");
        }
        _tree[node][GEQ] = _npts;
        dir=GEQ;
    }
    _tree[_npts][LT] =  -1;
    _tree[_npts][GEQ] =  -1;
    for (i = 0; i < _dimensions; i++ ) {
        _data[_npts][i] = v[i];
    }

    _npts++;


    i = _walkUpTree(_tree[_npts-1][PARENT], dir, _npts-1);
    if (i != _masterParent){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
        "Adding to KdTree failed\n");
    }

}

template  < typename T >
int KdTree < T > ::getNPoints() const
{
    return _npts;
}

template  < typename T >
void KdTree < T > ::getTreeNode(ndarray::Array < int,1,1 >  const &v,
                                int dex) const
{
    if(dex < 0 || dex >= _npts){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for tree information on point that does not exist\n");
    }

    if(v.getNumElements() != 4){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Need to pass a 4-element ndarray into KdTree.getTreeNode()\n");
    }

    v[0] = _tree[dex][DIMENSION];
    v[1] = _tree[dex][LT];
    v[2] = _tree[dex][GEQ];
    v[3] = _tree[dex][PARENT];
}

template  < typename T >
int KdTree < T > ::_testTree() const{

    if(_npts == 1 && _tree[0][PARENT] < 0 && _masterParent==0){
        // there is only one point in the tree
        return 1;
    }

    int i,j,output;
    std::vector < int >  isparent;

    output=1;

    for (i = 0; i < _npts; i++ ) isparent.push_back(0);

    j = 0;
    for (i = 0; i < _npts; i++ ) {
        if(_tree[i][PARENT] < 0)j++;
    }
    if(j != 1){
        std::cout << "_tree FAILURE " << j << " _masterParents\n";
        return 0;
    }

    for (i = 0; i < _npts; i++ ) {
        if(_tree[i][PARENT] >= 0){
            isparent[_tree[i][PARENT]]++;
        }
    }

    for (i = 0; i < _npts; i++ ) {
        if(isparent[i] > 2){
            std::cout << "_tree FAILURE " << i << " is parent to " << isparent[i] << "\n";
            return 0;
        }
    }


    for (i = 0; i < _npts; i++ ) {
        if(_tree[i][PARENT] >= 0) {
            if(_tree[_tree[i][PARENT]][LT] == i)j = LT;
            else j = GEQ;
            output = _walkUpTree(_tree[i][PARENT], j, i);

            if(output != _masterParent){
                return 0;
            }
        }
    }

    if(output != _masterParent) return 0;
    else return 1;
}

template  < typename T >
void KdTree < T > ::_organize(ndarray::Array < int,1,1 >  const &use,
                              int ct,
                              int parent,
                              int dir
                              )
{

    int i,j,k,l,idim,daughter;
    T mean,var,varbest;


    std::vector <  std::vector < T >   >  toSort;
    std::vector < T >  toSortElement;

    if(ct > 1){
      //below is code to choose the dimension on which the available points
      //have the greates variance.  This will be the dimension on which
      //the daughter node splits the data
        idim=0;
        varbest=-1.0;
        for (i = 0; i < _dimensions; i++ ) {
            mean = 0.0;
            var = 0.0;
            for (j = 0; j < ct; j++ ) {
                mean += _data[use[j]][i];
                var += _data[use[j]][i]*_data[use[j]][i];
            }
            mean = mean/double(ct);
            var = var/double(ct) - mean*mean;
            if(i == 0 || var > varbest ||
               (var == varbest && parent >= 0 && i > _tree[parent][DIMENSION])) {
                    idim = i;
                    varbest = var;
            }
        }//for(i = 0;i < _dimensions;i++ )

        //we need to sort the available data points according to their idim - th element
        //but we need to keep track of their original indices, so we can refer back to
        //their positions in _data.  Therefore, the code constructs a 2 - dimensional
        //std::vector.  The first dimension contains the element to be sorted.
        //The second dimension contains the original index information.
        //After sorting, the (rearranged) index information will be stored back in the
        //ndarray use[]

        toSortElement.push_back(_data[use[0]][idim]);
        toSortElement.push_back(T(use[0]));
        toSort.push_back(toSortElement);
        for(i = 1; i < ct; i++ ) {
            toSortElement[0] = _data[use[i]][idim];
            toSortElement[1] = T(use[i]);
            toSort.push_back(toSortElement);
        }

        std::stable_sort(toSort.begin(), toSort.end());

        k = ct/2;
        l = ct/2;
        while(k > 0 && toSort[k][0] == toSort[k - 1][0]) k--;

        while(l < ct - 1 && toSort[l][0] == toSort[ct/2][0]) l++;

        if((ct/2 - k) < (l - ct/2) || l == ct - 1) j = k;
        else j = l;

        for(i = 0; i < ct; i++ ) {
            use[i] = int(toSort[i][1]);
        }
        daughter = use[j];

        if(parent >= 0)_tree[parent][dir] = daughter;
        _tree[daughter][DIMENSION] = idim;
        _tree[daughter][PARENT] = parent;

        if(j < ct - 1){
            _organize(use[ndarray::view(j + 1,use.getSize < 0 > ())], ct - j - 1, daughter, GEQ);
        }
        else _tree[daughter][GEQ] =  -1;

        if(j > 0){
            _organize(use, j, daughter, LT);
        }
        else _tree[daughter][LT] =  -1;

    }//if(ct > 1)
    else{
        daughter = use[0];

        if(parent >= 0) _tree[parent][dir] = daughter;

        idim = _tree[parent][DIMENSION] + 1;

        if(idim >= _dimensions)idim = 0;

        _tree[daughter][DIMENSION] = idim;
        _tree[daughter][LT] =  - 1;
        _tree[daughter][GEQ] =  - 1;
        _tree[daughter][PARENT] = parent;

    }

    if(parent ==  - 1){
        _masterParent = daughter;
    }

}

template  < typename T >
int KdTree < T > ::_findNode(ndarray::Array < const T,1,1 >  const &v) const
{
    int consider,next,dim;

    dim = _tree[_masterParent][DIMENSION];

    if(v[dim] < _data[_masterParent][dim]) consider = _tree[_masterParent][LT];
    else consider = _tree[_masterParent][GEQ];

    next = consider;

    while(next >= 0){

        consider = next;

        dim = _tree[consider][DIMENSION];
        if(v[dim] < _data[consider][dim]) next = _tree[consider][LT];
        else next = _tree[consider][GEQ];

    }

    return consider;
}

template < typename T >
void KdTree < T > ::_lookForNeighbors(ndarray::Array < const T,1,1 >  const &v,
                                      int consider,
                                      int from) const
{

    int i,j,going;
    double dd;

    dd = _distance(v, _data[consider]);

    if(_neighborsFound < _neighborsWanted || dd < _neighborDistances[_neighborsWanted -1]) {

        for(j = 0; j < _neighborsFound && _neighborDistances[j] < dd; j++ );

        for(i = _neighborsWanted - 1; i > j; i-- ){
            _neighborDistances[i] = _neighborDistances[i - 1];
            _neighborCandidates[i] = _neighborCandidates[i - 1];
        }

        _neighborDistances[j] = dd;
        _neighborCandidates[j] = consider;

        if(_neighborsFound < _neighborsWanted) _neighborsFound++;
    }

    if(_tree[consider][PARENT] == from){
    //you came here from the parent

        i = _tree[consider][DIMENSION];
        dd = v[i] - _data[consider][i];
        if((dd <= _neighborDistances[_neighborsFound - 1] || _neighborsFound < _neighborsWanted)
            && _tree[consider][LT] >= 0){

              _lookForNeighbors(v, _tree[consider][LT], consider);
        }

        dd = _data[consider][i] - v[i];
        if((dd <= _neighborDistances[_neighborsFound - 1] || _neighborsFound < _neighborsWanted)
            && _tree[consider][GEQ] >= 0){

            _lookForNeighbors(v, _tree[consider][GEQ], consider);
        }
    }
    else{
    //you came here from one of the branches

        //descend the other branch
        if(_tree[consider][LT] == from){
            going = GEQ;
        }
        else{
            going = LT;
        }

        j = _tree[consider][going];

        if(j >= 0){
            i = _tree[consider][DIMENSION];

            if(going == 1) dd = v[i] - _data[consider][i];
            else dd = _data[consider][i] - v[i];

            if(dd <= _neighborDistances[_neighborsFound - 1] || _neighborsFound < _neighborsWanted) {
                  _lookForNeighbors(v, j, consider);
            }
        }

        //ascend to the parent
        if(_tree[consider][PARENT] >= 0) {
              _lookForNeighbors(v, _tree[consider][PARENT], consider);
        }
    }
}

template  < typename T >
int KdTree < T > ::_walkUpTree(int target,
                               int dir,
                               int root) const
{
    //target is the node that you are examining now
    //dir is where you came from
    //root is the ultimate point from which you started

    int i,output;

    output = 1;

    if(dir == LT){
        if(_data[root][_tree[target][DIMENSION]] >= _data[target][_tree[target][DIMENSION]]){
            return 0;
        }
    }
    else{
        if(_data[root][_tree[target][DIMENSION]] < _data[target][_tree[target][DIMENSION]]) {
            return 0;
        }
    }

    if(_tree[target][PARENT] >= 0){
        if(_tree[_tree[target][PARENT]][LT] == target)i = LT;
        else i = GEQ;
        output = output*_walkUpTree(_tree[target][PARENT],i,root);

    }
    else{
        output = output*target;
        //so that it will return _masterParent
        //make sure everything is connected to _masterParent
    }
    return output;

}

template  < typename T >
void KdTree < T > ::removePoint(int target)
{

    if(target < 0 || target >= _npts){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You are trying to remove a point that doesn't exist from KdTree\n");
    }

    if(_npts==1){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "There is only one point left in this KdTree.  You cannot call removePoint\n");
    }

    int nl,nr,i,j,k,side;
    int root;

    nl = 0;
    nr = 0;
    if(_tree[target][LT] >= 0){
        nl++ ;
        _count(_tree[target][LT], &nl);
    }

    if(_tree[target][GEQ] >= 0){
        nr++;
        _count(_tree[target][GEQ], &nr);
    }

    if(nl == 0 && nr == 0){

        k = _tree[target][PARENT];

          if(_tree[k][LT] == target) _tree[k][LT] =  - 1;
          else if(_tree[k][GEQ] == target) _tree[k][GEQ] =  - 1;

    }//if target is terminal
    else if((nl == 0 && nr > 0) || (nr == 0 && nl > 0)){

         if(nl == 0) side = GEQ;
         else side = LT;

         k = _tree[target][PARENT];
         if(k >= 0){

             if(_tree[k][LT] == target){
                 _tree[k][LT] = _tree[target][side];
                 _tree[_tree[k][LT]][PARENT] = k;

         }
             else{
                 _tree[k][GEQ] = _tree[target][side];
                 _tree[_tree[k][GEQ]][PARENT] = k;
             }
        }
        else{
            _masterParent = _tree[target][side];
            _tree[_tree[target][side]][PARENT] =  - 1;
        }
    }//if only one side is populated
    else{

        if(nl > nr)side = LT;
        else side = GEQ;

         k = _tree[target][PARENT];
         if(k < 0){
             _masterParent = _tree[target][side];
             _tree[_masterParent][PARENT] =  - 1;
          }
          else{
              if(_tree[k][LT] == target){
                  _tree[k][LT] = _tree[target][side];
                  _tree[_tree[k][LT]][PARENT] = k;
              }
              else{
                   _tree[k][GEQ] = _tree[target][side];
                   _tree[_tree[k][GEQ]][PARENT] = k;
              }
          }

         root = _tree[target][3 - side];

         _descend(root);
    }//if both sides are populated

    if(target < _npts - 1){
         for(i = target + 1;i < _npts;i++ ){
             for(j = 0; j < 4; j++ ) _tree[i - 1][j] = _tree[i][j];

             for(j = 0; j < _dimensions; j++ ) _data[i - 1][j] = _data[i][j];
        }

        for(i = 0; i < _npts; i++ ){
            for(j = 1; j < 4; j++ ){
                if(_tree[i][j] > target) _tree[i][j]--;
             }
        }

        if(_masterParent > target)_masterParent-- ;
    }
    _npts--;

    i = _testTree();
    if (i == 0) {
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
        "Subtracting from KdTree failed\n");
    }


}

template  < typename T >
void KdTree < T > ::_count(int where, int *ct) const
{
    //a way to count the number of vital elements on a given branch

    if(_tree[where][LT] >= 0) {
        ct[0]++;
        _count(_tree[where][LT], ct);
    }
    if(_tree[where][GEQ] >= 0) {
        ct[0]++;
        _count(_tree[where][GEQ], ct);
    }
}

template  < typename T >
void KdTree < T > ::_reassign(int target)
{

    int where,dir,k;

    where = _masterParent;
    if(_data[target][_tree[where][DIMENSION]] < _data[where][_tree[where][DIMENSION]]) dir = LT;
    else dir = GEQ;

    k = _tree[where][dir];
    while(k >= 0) {
        where = k;
        if(_data[target][_tree[where][DIMENSION]] < _data[where][_tree[where][DIMENSION]]) dir = LT;
        else dir = GEQ;
        k = _tree[where][dir];
    }

    _tree[where][dir] = target;
    _tree[target][PARENT] = where;
    _tree[target][LT] =  - 1;
    _tree[target][GEQ] =  - 1;
    _tree[target][DIMENSION] = _tree[where][DIMENSION] + 1;

    if(_tree[target][DIMENSION] == _dimensions) _tree[target][DIMENSION] = 0;
}

template  < typename T >
void KdTree < T > ::_descend(int root)
{

    if(_tree[root][LT] >= 0) _descend(_tree[root][LT]);
    if(_tree[root][GEQ] >= 0) _descend(_tree[root][GEQ]);

    _reassign(root);
}

template  < typename T >
double KdTree < T > ::_distance(ndarray::Array < const T,1,1 >  const &p1,
                                ndarray::Array < const T,1,1 >  const &p2) const
{

    int i,dd;
    double ans;
    ans = 0.0;
    dd = p1.template getSize< 0 >();

    for(i = 0;i < dd;i++ ) ans += (p1[i] - p2[i])*(p1[i] - p2[i]);

    return ::sqrt(ans);

}


template  < typename T >
GaussianProcess < T > ::GaussianProcess(ndarray::Array < T,2,2 >  const &dataIn,
                                        ndarray::Array < T,1,1 >  const &ff,
                                        std::shared_ptr <  Covariogram < T >   >  const &covarIn)

{
    int i;

    _covariogram = covarIn;

    _npts = dataIn.template getSize < 0 > ();
    _dimensions = dataIn.template getSize < 1 > ();

    _room = _npts;
    _roomStep = 5000;

    _nFunctions = 1;
    _function = allocate(ndarray::makeVector(_npts,1));

    if(ff.getNumElements() != _npts){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You did not pass in the same number of data points as function values\n");
    }

    for(i = 0; i < _npts; i++ ) _function[i][0] = ff[i];

    _krigingParameter = T(1.0);
    _lambda = T(1.0e-5);

    _useMaxMin = 0;


    _kdTree.Initialize(dataIn);

    _npts = _kdTree.getNPoints();

}

template  < typename T >
GaussianProcess < T > ::GaussianProcess(ndarray::Array < T,2,2 >  const &dataIn,
                                        ndarray::Array < T,1,1 >  const &mn,
                                        ndarray::Array < T,1,1 >  const &mx,
                                        ndarray::Array < T,1,1 >  const &ff,
                                        std::shared_ptr <  Covariogram < T >   >  const &covarIn
                                        )
{

    int i,j;
    ndarray::Array < T,2,2 >  normalizedData;

    _covariogram = covarIn;

    _npts = dataIn.template getSize < 0 > ();
    _dimensions = dataIn.template getSize < 1 > ();
    _room = _npts;
    _roomStep = 5000;

    if(ff.getNumElements() != _npts){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You did not pass in the same number of data points as function values\n");
    }

    if(mn.getNumElements() != _dimensions || mx.getNumElements() != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your min/max values have different dimensionality than your data points\n");
    }

    _krigingParameter = T(1.0);

    _lambda = T(1.0e-5);
    _krigingParameter = T(1.0);

    _max = allocate(ndarray::makeVector(_dimensions));
    _min = allocate(ndarray::makeVector(_dimensions));
    _max.deep() = mx;
    _min.deep() = mn;
    _useMaxMin = 1;
    normalizedData = allocate(ndarray::makeVector(_npts, _dimensions));
    for(i = 0; i < _npts; i++ ) {
        for(j = 0; j < _dimensions; j++ ) {
            normalizedData[i][j] = (dataIn[i][j] - _min[j])/(_max[j] - _min[j]);
            //note the normalization by _max - _min in each dimension
        }
    }

    _kdTree.Initialize(normalizedData);

    _npts = _kdTree.getNPoints();
    _nFunctions = 1;
    _function = allocate(ndarray::makeVector(_npts, 1));
    for(i = 0; i < _npts; i++ )_function[i][0] = ff[i];
}

template  < typename T >
GaussianProcess < T > ::GaussianProcess(ndarray::Array < T,2,2 >  const &dataIn,
                                        ndarray::Array < T,2,2 >  const &ff,
                                        std::shared_ptr <Covariogram < T > >  const &covarIn)
{

    _covariogram = covarIn;

    _npts = dataIn.template getSize < 0 > ();
    _dimensions = dataIn.template getSize < 1 > ();

    _room = _npts;
    _roomStep = 5000;

    if(ff.template getSize < 0 > () != _npts){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You did not pass in the same number of data points as function values\n");
    }

    _nFunctions = ff.template getSize < 1 > ();
    _function = allocate(ndarray::makeVector(_npts, _nFunctions));
    _function.deep() = ff;

    _krigingParameter = T(1.0);

    _lambda = T(1.0e-5);

    _useMaxMin = 0;

    _kdTree.Initialize(dataIn);

    _npts = _kdTree.getNPoints();
}

template  < typename T >
GaussianProcess < T > ::GaussianProcess(ndarray::Array < T,2,2 >  const &dataIn,
                                    ndarray::Array < T,1,1 >  const &mn,
                                    ndarray::Array < T,1,1 >  const &mx,
                                    ndarray::Array < T,2,2 >  const &ff,
                                    std::shared_ptr <Covariogram < T > >  const &covarIn
                                    )
{

    int i,j;
    ndarray::Array < T,2,2 >  normalizedData;

    _covariogram = covarIn;

    _npts = dataIn.template getSize < 0 > ();
    _dimensions = dataIn.template getSize < 1 > ();

    _room = _npts;
    _roomStep = 5000;

    if(ff.template getSize < 0 > () != _npts){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You did not pass in the same number of data points as function values\n");
    }

    if(mn.getNumElements() != _dimensions || mx.getNumElements() != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your min/max values have different dimensionality than your data points\n");
    }

    _krigingParameter = T(1.0);

    _lambda = T(1.0e-5);
    _krigingParameter = T(1.0);

    _max = allocate(ndarray::makeVector(_dimensions));
    _min = allocate(ndarray::makeVector(_dimensions));
    _max.deep() = mx;
    _min.deep() = mn;
    _useMaxMin = 1;
    normalizedData = allocate(ndarray::makeVector(_npts,_dimensions));
    for(i = 0; i < _npts; i++ ) {
        for(j = 0; j < _dimensions; j++ ) {
            normalizedData[i][j] = (dataIn[i][j] - _min[j])/(_max[j] - _min[j]);
            //note the normalization by _max - _min in each dimension
        }
    }

    _kdTree.Initialize(normalizedData);
    _npts = _kdTree.getNPoints();
    _nFunctions = ff.template getSize < 1 > ();
    _function = allocate(ndarray::makeVector(_npts, _nFunctions));
    _function.deep() = ff;
}


template < typename T >
int GaussianProcess < T > ::getNPoints() const{
    return _npts;
}


template < typename T >
int GaussianProcess < T > ::getDim() const{
    return _dimensions;
}

template  < typename T >
void GaussianProcess < T > ::getData(ndarray::Array <T, 2, 2> pts, ndarray::Array<T, 1, 1> fn,
                                     ndarray::Array <int, 1, 1 > indices) const{

    if(_nFunctions != 1){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your function value array does not have enough room for all of the functions "
                          "in your GaussianProcess.\n");
    }

    if(pts.template getSize < 1 > () != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your pts array is constructed for points of the wrong dimensionality.\n");
    }

    if(pts.template getSize < 0 > () != indices.getNumElements()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You did not put enough room in your pts array to fit all the points "
                          "you asked for in your indices array.\n");
    }

    if(fn.template getSize < 0 > () != indices.getNumElements()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You did not provide enough room in your function value array "
                          "for all of the points you requested in your indices array.\n");
    }

    int i,j;
    for(i = 0; i < indices.template getSize < 0 > (); i++){
        pts[i] = _kdTree.getData(indices[i]);  // do this first in case one of the indices is invalid.
                                               // _kdTree.getData() will raise an exception in that case
        fn[i] = _function[indices[i]][0];
        if(_useMaxMin == 1){
            for(j = 0; j < _dimensions; j ++){
                pts[i][j] *= (_max[j] - _min[j]);
                pts[i][j] += _min[j];
            }
        }
    }

}


template  < typename T >
void GaussianProcess < T > ::getData(ndarray::Array <T, 2, 2> pts, ndarray::Array<T, 2, 2> fn,
                                     ndarray::Array <int, 1, 1 > indices) const{

    if(fn.template getSize < 1 > () != _nFunctions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your function value array does not have enough room for all of the functions "
                          "in your GaussianProcess.\n");
    }

    if(pts.template getSize < 1 > () != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your pts array is constructed for points of the wrong dimensionality.\n");
    }

    if(pts.template getSize < 0 > () != indices.getNumElements()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You did not put enough room in your pts array to fit all the points "
                          "you asked for in your indices array.\n");
    }

    if(fn.template getSize < 0 > () != indices.getNumElements()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You did not provide enough room in your function value array "
                          "for all of the points you requested in your indices array.\n");
    }

    int i,j;
    for(i = 0; i < indices.template getSize < 0 > (); i++){
        pts[i] = _kdTree.getData(indices[i]);  // do this first in case one of the indices is invalid.
                                               // _kdTree.getData() will raise an exception in that case
        for(j = 0; j < _nFunctions; j ++){
            fn[i][j] = _function[indices[i]][j];
        }
        if(_useMaxMin == 1){
            for(j = 0; j < _dimensions; j ++){
                pts[i][j] *= (_max[j] - _min[j]);
                pts[i][j] += _min[j];
            }
        }
    }

}


template  < typename T >
T GaussianProcess < T > ::interpolate(ndarray::Array < T,1,1 >  variance,
                                      ndarray::Array < T,1,1 >  const &vin,
                                      int numberOfNeighbors) const
{

    if(_nFunctions > 1){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You need to call the version of GaussianProcess.interpolate() "
                          "that accepts mu and variance arrays (which it populates with results). "
                          "You are interpolating more than one function.");
    }

    if(numberOfNeighbors <= 0){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for zero or negative number of neighbors\n");
    }

    if(numberOfNeighbors > _kdTree.getNPoints()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for more neighbors than you have data points\n");
    }

    if(variance.getNumElements() != _nFunctions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your variance array is the incorrect size for the number "
                          "of functions you are trying to interpolate\n");
    }

    if(vin.getNumElements() != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You are interpolating at a point with different dimensionality than you data\n");
    }

    int i,j;
    T fbar,mu;

    ndarray::Array < T,1,1 >  covarianceTestPoint;
    ndarray::Array < int,1,1 >  neighbors;
    ndarray::Array < double,1,1 >  neighborDistances,vv;

    Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >  covariance,bb,xx;
    Eigen::LDLT < Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >   >  ldlt;


    _timer.start();

    bb.resize(numberOfNeighbors, 1);
    xx.resize(numberOfNeighbors, 1);

    covariance.resize(numberOfNeighbors,numberOfNeighbors);

    covarianceTestPoint = allocate(ndarray::makeVector(numberOfNeighbors));

    neighbors = allocate(ndarray::makeVector(numberOfNeighbors));

    neighborDistances = allocate(ndarray::makeVector(numberOfNeighbors));

    vv = allocate(ndarray::makeVector(_dimensions));
    if(_useMaxMin == 1){
        //if you constructed this Gaussian process with minimum and maximum
        //values for the dimensions of your parameter space,
        //the point you are interpolating must be scaled to match the data so
        //that the selected nearest neighbors are appropriate

        for(i = 0; i < _dimensions; i++ ) vv[i] = (vin[i] - _min[i])/(_max[i] - _min[i]);
    }
    else{
        vv = vin;
    }


    _kdTree.findNeighbors(neighbors, neighborDistances, vv, numberOfNeighbors);

    _timer.addToSearch();

    fbar = 0.0;
    for(i = 0; i < numberOfNeighbors; i++ )fbar += _function[neighbors[i]][0];
    fbar = fbar/double(numberOfNeighbors);

    for(i = 0; i < numberOfNeighbors; i++ ){
        covarianceTestPoint[i] = (*_covariogram)(vv, _kdTree.getData(neighbors[i]));

        covariance(i,i) = (*_covariogram)(_kdTree.getData(neighbors[i]), _kdTree.getData(neighbors[i]))
                         + _lambda;

        for(j = i + 1; j < numberOfNeighbors; j++ ){
            covariance(i,j) = (*_covariogram)(_kdTree.getData(neighbors[i]),
                                              _kdTree.getData(neighbors[j]));
            covariance(j,i) = covariance(i, j);
        }
    }

    _timer.addToIteration();

    //use Eigen's ldlt solver in place of matrix inversion (for speed purposes)
    ldlt.compute(covariance);

    for(i = 0; i < numberOfNeighbors; i++) bb(i,0) = _function[neighbors[i]][0] - fbar;

    xx = ldlt.solve(bb);
    _timer.addToEigen();

    mu = fbar;

    for(i = 0; i < numberOfNeighbors; i++ ){
        mu += covarianceTestPoint[i]*xx(i, 0);
    }

    _timer.addToIteration();

    variance(0) = (*_covariogram)(vv, vv) + _lambda;

    for(i = 0; i < numberOfNeighbors; i++ ) bb(i) = covarianceTestPoint[i];

    xx = ldlt.solve(bb);

    for(i = 0; i < numberOfNeighbors; i++ ){
        variance(0) -= covarianceTestPoint[i]*xx(i, 0);
    }

    variance(0) = variance(0)*_krigingParameter;

    _timer.addToVariance();
    _timer.addToTotal(1);

    return mu;
}

template  < typename T >
void GaussianProcess < T > ::interpolate(ndarray::Array < T,1,1 >  mu,
                                         ndarray::Array < T,1,1 >  variance,
                                         ndarray::Array < T,1,1 >  const &vin,
                                         int numberOfNeighbors) const
{


    if(numberOfNeighbors <= 0){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for zero or negative number of neighbors\n");
    }

    if(numberOfNeighbors > _kdTree.getNPoints()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for more neighbors than you have data points\n");
    }

    if(vin.getNumElements() != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You are interpolating at a point with different dimensionality than you data\n");
    }

    if(mu.getNumElements() != _nFunctions || variance.getNumElements() != _nFunctions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your mu and/or var arrays are improperly sized for the number of functions "
                          "you are interpolating\n");
    }

    int i,j,ii;
    T fbar;

    ndarray::Array < T,1,1 >  covarianceTestPoint;
    ndarray::Array < int,1,1 >  neighbors;
    ndarray::Array < double,1,1 >  neighborDistances,vv;

    Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >  covariance,bb,xx;
    Eigen::LDLT < Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >   >  ldlt;

    _timer.start();


     bb.resize(numberOfNeighbors,1);
     xx.resize(numberOfNeighbors,1);
     covariance.resize(numberOfNeighbors,numberOfNeighbors);
     covarianceTestPoint = allocate(ndarray::makeVector(numberOfNeighbors));
     neighbors = allocate(ndarray::makeVector(numberOfNeighbors));
     neighborDistances = allocate(ndarray::makeVector(numberOfNeighbors));

     vv = allocate(ndarray::makeVector(_dimensions));


    if(_useMaxMin == 1) {
        //if you constructed this Gaussian process with minimum and maximum
        //values for the dimensions of your parameter space,
        //the point you are interpolating must be scaled to match the data so
        //that the selected nearest neighbors are appropriate

        for(i = 0; i < _dimensions; i++ )vv[i] = (vin[i] - _min[i])/(_max[i] - _min[i]);
    }
    else {
        vv = vin;
    }


    _kdTree.findNeighbors(neighbors, neighborDistances, vv, numberOfNeighbors);

    _timer.addToSearch();

    for(i = 0; i < numberOfNeighbors; i++ ) {
        covarianceTestPoint[i] = (*_covariogram)(vv, _kdTree.getData(neighbors[i]));

        covariance(i,i) = (*_covariogram)(_kdTree.getData(neighbors[i]), _kdTree.getData(neighbors[i]))
                           + _lambda;

        for(j = i + 1; j < numberOfNeighbors; j++ ){
            covariance(i,j) = (*_covariogram)(_kdTree.getData(neighbors[i]),
                                              _kdTree.getData(neighbors[j]));
            covariance(j,i) = covariance(i, j);
        }
    }

    _timer.addToIteration();

    //use Eigen's ldlt solver in place of matrix inversion (for speed purposes)
    ldlt.compute(covariance);

    for(ii = 0; ii < _nFunctions; ii++ ) {

          fbar = 0.0;
          for(i = 0; i < numberOfNeighbors; i++ )fbar += _function[neighbors[i]][ii];
          fbar = fbar/double(numberOfNeighbors);

          for(i = 0; i < numberOfNeighbors; i++ )bb(i,0) = _function[neighbors[i]][ii] - fbar;
          xx = ldlt.solve(bb);

          mu[ii] = fbar;

          for(i = 0; i < numberOfNeighbors; i++ ) {
            mu[ii] += covarianceTestPoint[i]*xx(i, 0);
          }

    }//ii = 0 through _nFunctions

    _timer.addToEigen();

    variance[0] = (*_covariogram)(vv, vv) + _lambda;

    for(i = 0; i < numberOfNeighbors; i++ )bb(i) = covarianceTestPoint[i];

    xx = ldlt.solve(bb);

    for(i = 0; i < numberOfNeighbors; i++ ) {
        variance[0] -= covarianceTestPoint[i]*xx(i, 0);
    }
    variance[0] = variance[0]*_krigingParameter;


    for(i = 1; i < _nFunctions; i++ )variance[i] = variance[0];

    _timer.addToVariance();
    _timer.addToTotal(1);
}


template  < typename T >
T GaussianProcess < T > ::selfInterpolate(ndarray::Array < T,1,1 >  variance,
                                          int dex,
                                          int numberOfNeighbors) const
{

    if(_nFunctions > 1){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You need to call the version of GaussianProcess.selfInterpolate() "
                          "that accepts mu and variance arrays (which it populates with results). "
                          "You are interpolating more than one function.");
    }

    if(variance.getNumElements() != _nFunctions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your variance array is the incorrect size for the number "
                          "of functions you are trying to interpolate\n");
    }

    if(numberOfNeighbors <= 0){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for zero or negative number of neighbors\n");
    }

    if(numberOfNeighbors + 1 > _kdTree.getNPoints()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for more neighbors than you have data points\n");
    }

    if(dex < 0 || dex >=_kdTree.getNPoints()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked to self interpolate on a point that does not exist\n");
    }

    int i,j;
    T fbar,mu;

    ndarray::Array < T,1,1 >  covarianceTestPoint;
    ndarray::Array < int,1,1 >  selfNeighbors;
    ndarray::Array < double,1,1 >  selfDistances;
    ndarray::Array < int,1,1 >  neighbors;
    ndarray::Array < double,1,1 >  neighborDistances;

    Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >  covariance,bb,xx;
    Eigen::LDLT < Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >   >  ldlt;

    _timer.start();

    bb.resize(numberOfNeighbors, 1);
    xx.resize(numberOfNeighbors, 1);
    covariance.resize(numberOfNeighbors,numberOfNeighbors);
    covarianceTestPoint = allocate(ndarray::makeVector(numberOfNeighbors));
    neighbors = allocate(ndarray::makeVector(numberOfNeighbors));
    neighborDistances = allocate(ndarray::makeVector(numberOfNeighbors));

    selfNeighbors = allocate(ndarray::makeVector(numberOfNeighbors + 1));
    selfDistances = allocate(ndarray::makeVector(numberOfNeighbors + 1));

    //we don't use _useMaxMin because the data has already been normalized


    _kdTree.findNeighbors(selfNeighbors, selfDistances, _kdTree.getData(dex),
                          numberOfNeighbors + 1);

    _timer.addToSearch();

    if(selfNeighbors[0]!= dex) {
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
        "Nearest neighbor search in selfInterpolate did not find self\n");
    }

    //SelfNeighbors[0] will be the point itself (it is its own nearest neighbor)
    //We discard that for the interpolation calculation
    //
    //If you do not wish to do this, simply call the usual ::interpolate() method instead of
    //::selfInterpolate()
    for(i = 0; i < numberOfNeighbors; i++ ){
        neighbors[i] = selfNeighbors[i + 1];
        neighborDistances[i] = selfDistances[i + 1];
    }

    fbar = 0.0;
    for(i = 0; i < numberOfNeighbors; i++ ) fbar += _function[neighbors[i]][0];
    fbar = fbar/double(numberOfNeighbors);

    for(i = 0; i < numberOfNeighbors; i++ ){
        covarianceTestPoint[i] = (*_covariogram)(_kdTree.getData(dex),
                                                 _kdTree.getData(neighbors[i]));

        covariance(i, i) = (*_covariogram)(_kdTree.getData(neighbors[i]),
                                           _kdTree.getData(neighbors[i])) + _lambda;

      for(j = i + 1; j < numberOfNeighbors; j++ ) {
          covariance(i, j) = (*_covariogram)(_kdTree.getData(neighbors[i]),
                                             _kdTree.getData(neighbors[j]));
          covariance(j, i) = covariance(i ,j);
        }
    }
    _timer.addToIteration();

    //use Eigen's ldlt solver in place of matrix inversion (for speed purposes)
    ldlt.compute(covariance);


    for(i = 0; i < numberOfNeighbors;i++ ) bb(i, 0) = _function[neighbors[i]][0] - fbar;
    xx = ldlt.solve(bb);
    _timer.addToEigen();

    mu = fbar;

    for(i = 0; i < numberOfNeighbors; i++ ) {
        mu += covarianceTestPoint[i]*xx(i, 0);
    }


    variance(0) = (*_covariogram)(_kdTree.getData(dex), _kdTree.getData(dex)) + _lambda;

    for(i = 0; i < numberOfNeighbors; i++ )bb(i) = covarianceTestPoint[i];

    xx = ldlt.solve(bb);

    for(i = 0; i < numberOfNeighbors; i++ ){
        variance(0) -= covarianceTestPoint[i]*xx(i,0);
    }

    variance(0) = variance(0)*_krigingParameter;
    _timer.addToVariance();
    _timer.addToTotal(1);

    return mu;
}

template  < typename T >
void GaussianProcess < T > ::selfInterpolate(ndarray::Array < T,1,1 >  mu,
                                             ndarray::Array < T,1,1 >  variance,
                                             int dex,
                                             int numberOfNeighbors) const{

    if(mu.getNumElements() != _nFunctions || variance.getNumElements() != _nFunctions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your mu and/or var arrays are improperly sized for the number of functions "
                          "you are interpolating\n");
    }

    if(numberOfNeighbors <= 0){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for zero or negative number of neighbors\n");
    }

    if(numberOfNeighbors + 1 > _kdTree.getNPoints()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked for more neighbors than you have data points\n");
    }

    if(dex < 0 || dex >=_kdTree.getNPoints()){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Asked to self interpolate on a point that does not exist\n");
    }

    int i,j,ii;
    T fbar;

    ndarray::Array < T,1,1 >  covarianceTestPoint;
    ndarray::Array < int,1,1 >  selfNeighbors;
    ndarray::Array < double,1,1 >  selfDistances;
    ndarray::Array < int,1,1 >  neighbors;
    ndarray::Array < double,1,1 >  neighborDistances;

    Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >  covariance,bb,xx;
    Eigen::LDLT < Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >   >  ldlt;

    _timer.start();

    bb.resize(numberOfNeighbors, 1);
    xx.resize(numberOfNeighbors, 1);
    covariance.resize(numberOfNeighbors,numberOfNeighbors);
    covarianceTestPoint = allocate(ndarray::makeVector(numberOfNeighbors));
    neighbors = allocate(ndarray::makeVector(numberOfNeighbors));
    neighborDistances = allocate(ndarray::makeVector(numberOfNeighbors));

    selfNeighbors = allocate(ndarray::makeVector(numberOfNeighbors + 1));
    selfDistances = allocate(ndarray::makeVector(numberOfNeighbors + 1));

    //we don't use _useMaxMin because the data has already been normalized


    _kdTree.findNeighbors(selfNeighbors, selfDistances, _kdTree.getData(dex),
                          numberOfNeighbors + 1);

    _timer.addToSearch();

    if(selfNeighbors[0]!= dex) {

        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
        "Nearest neighbor search in selfInterpolate did not find self\n");
    }

    //SelfNeighbors[0] will be the point itself (it is its own nearest neighbor)
    //We discard that for the interpolation calculation
    //
    //If you do not wish to do this, simply call the usual ::interpolate() method instead of
    //::selfInterpolate()
    for(i = 0; i < numberOfNeighbors; i++ ) {
        neighbors[i] = selfNeighbors[i + 1];
        neighborDistances[i] = selfDistances[i + 1];
    }



    for(i = 0; i < numberOfNeighbors; i++ ) {
        covarianceTestPoint[i] = (*_covariogram)(_kdTree.getData(dex),_kdTree.getData(neighbors[i]));

        covariance(i, i) = (*_covariogram)(_kdTree.getData(neighbors[i]), _kdTree.getData(neighbors[i]))
                           + _lambda;

        for(j = i + 1; j < numberOfNeighbors; j++ ) {
            covariance(i, j) = (*_covariogram)(_kdTree.getData(neighbors[i]),
                                               _kdTree.getData(neighbors[j]));
            covariance(j, i) = covariance(i, j);
        }
    }
    _timer.addToIteration();


    //use Eigen's ldlt solver in place of matrix inversion (for speed purposes)
    ldlt.compute(covariance);

    for(ii = 0; ii < _nFunctions; ii++ ) {

        fbar = 0.0;
        for(i = 0; i < numberOfNeighbors; i++ )fbar += _function[neighbors[i]][ii];
        fbar = fbar/double(numberOfNeighbors);

        for(i = 0; i < numberOfNeighbors; i++ )bb(i,0) = _function[neighbors[i]][ii] - fbar;
        xx = ldlt.solve(bb);

        mu[ii] = fbar;

        for(i = 0; i < numberOfNeighbors; i++ ){
            mu[ii] += covarianceTestPoint[i]*xx(i,0);
        }
    }//ii = 0 through _nFunctions

    _timer.addToEigen();

    variance[0] = (*_covariogram)(_kdTree.getData(dex), _kdTree.getData(dex)) + _lambda;

    for(i = 0; i < numberOfNeighbors; i++ )bb(i) = covarianceTestPoint[i];

    xx = ldlt.solve(bb);

    for(i = 0; i < numberOfNeighbors; i++ ) {
        variance[0] -= covarianceTestPoint[i]*xx(i,0);
    }

    variance[0] = variance[0]*_krigingParameter;

    for(i = 1; i < _nFunctions; i++ )variance[i] = variance[0];

    _timer.addToVariance();
    _timer.addToTotal(1);
}


template < typename T >
void GaussianProcess < T > ::batchInterpolate(ndarray::Array < T,1,1 >  mu,
                                              ndarray:: Array < T,1,1 >  variance,
                                              ndarray::Array < T,2,2 >  const &queries) const
{

    int i,j,ii,nQueries;

    nQueries = queries.template getSize < 0 > ();

    if(_nFunctions != 1){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your mu and variance arrays do not have room for all of the functions "
                          "as you are trying to interpolate\n");
    }

    if(mu.getNumElements() != nQueries || variance.getNumElements() != nQueries){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your mu and variance arrays do not have room for all of the points "
                          "at which you are trying to interpolate your function.\n");
    }

    if(queries.template getSize < 1 > () != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "The points you passed to batchInterpolate are of the wrong "
                          "dimensionality for your Gaussian Process\n");
    }

    T fbar;
    Eigen::Matrix  < T,Eigen::Dynamic,Eigen::Dynamic >  batchCovariance,batchbb,batchxx;
    Eigen::Matrix  < T,Eigen::Dynamic,Eigen::Dynamic >  queryCovariance;
    Eigen::LDLT < Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >   >  ldlt;

    ndarray::Array < T,1,1 >  v1;

    _timer.start();


    v1 = allocate(ndarray::makeVector(_dimensions));
    batchbb.resize(_npts, 1);
    batchxx.resize(_npts, 1);
    batchCovariance.resize(_npts, _npts);
    queryCovariance.resize(_npts, 1);


    for(i = 0; i < _npts; i++ ) {

        batchCovariance(i, i) = (*_covariogram)(_kdTree.getData(i), _kdTree.getData(i)) + _lambda;
        for(j = i + 1; j < _npts; j++ ) {
            batchCovariance(i, j) = (*_covariogram)(_kdTree.getData(i), _kdTree.getData(j));
            batchCovariance(j, i) = batchCovariance(i, j);
        }
    }
    _timer.addToIteration();

    ldlt.compute(batchCovariance);

    fbar = 0.0;
    for(i = 0; i < _npts; i++ ) {
        fbar += _function[i][0];
    }
    fbar = fbar/T(_npts);

    for(i = 0; i < _npts; i++ ){
        batchbb(i, 0) = _function[i][0] - fbar;
    }
    batchxx = ldlt.solve(batchbb);
    _timer.addToEigen();


    for(ii = 0; ii < nQueries; ii++ ) {
        for(i = 0; i < _dimensions; i++ )v1[i] = queries[ii][i];
        if(_useMaxMin == 1) {
            for(i = 0; i < _dimensions; i++ )v1[i] = (v1[i] - _min[i])/(_max[i] - _min[i]);
        }
        mu(ii) = fbar;
        for(i = 0; i < _npts; i++ ){
            mu(ii) += batchxx(i)*(*_covariogram)(v1, _kdTree.getData(i));
        }
    }
    _timer.addToIteration();

    for(ii = 0; ii < nQueries; ii++ ) {
        for(i = 0; i < _dimensions; i++ )v1[i] = queries[ii][i];
        if(_useMaxMin == 1) {
            for(i = 0; i < _dimensions; i++ )v1[i] = (v1[i] - _min[i])/(_max[i] - _min[i]);
        }

        for(i = 0; i < _npts; i++ ) {
            batchbb(i, 0) = (*_covariogram)(v1, _kdTree.getData(i));
            queryCovariance(i, 0) = batchbb(i, 0);
        }
        batchxx = ldlt.solve(batchbb);

        variance[ii] = (*_covariogram)(v1, v1) + _lambda;

        for(i = 0; i < _npts; i++ ){
            variance[ii] -= queryCovariance(i, 0)*batchxx(i);
        }

        variance[ii] = variance[ii]*_krigingParameter;

    }

    _timer.addToVariance();
    _timer.addToTotal(nQueries);
}

template < typename T >
void GaussianProcess < T > ::batchInterpolate(ndarray::Array < T,2,2 >  mu,
                                              ndarray:: Array < T,2,2 >  variance,
                                              ndarray::Array < T,2,2 >  const &queries) const
{

    int i,j,ii,nQueries,ifn;

    nQueries = queries.template getSize < 0 > ();

    if(mu.template getSize < 0 > () != nQueries || variance.template getSize < 0 > () != nQueries){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your output arrays do not have room for all of the points at which "
                          "you are interpolating your functions.\n");
    }

    if(mu.template getSize < 1 > () != _nFunctions || variance.template getSize < 1 > () != _nFunctions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your output arrays do not have room for all of the functions you are "
                          "interpolating\n");
    }

    if(queries.template getSize < 1 > () != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "The points at which you are interpolating your functions have the "
                          "wrong dimensionality.\n");
    }

    T fbar;
    Eigen::Matrix  < T,Eigen::Dynamic,Eigen::Dynamic >  batchCovariance,batchbb,batchxx;
    Eigen::Matrix  < T,Eigen::Dynamic,Eigen::Dynamic >  queryCovariance;
    Eigen::LDLT < Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >   >  ldlt;

    ndarray::Array < T,1,1 >  v1;

    _timer.start();

    v1 = allocate(ndarray::makeVector(_dimensions));
    batchbb.resize(_npts, 1);
    batchxx.resize(_npts, 1);
    batchCovariance.resize(_npts, _npts);
    queryCovariance.resize(_npts, 1);

    for(i = 0; i < _npts; i++ ){
        batchCovariance(i, i) = (*_covariogram)(_kdTree.getData(i), _kdTree.getData(i)) + _lambda;
        for(j = i + 1; j < _npts; j++ ) {
            batchCovariance(i, j) = (*_covariogram)(_kdTree.getData(i), _kdTree.getData(j));
            batchCovariance(j, i) = batchCovariance(i, j);
        }
    }

    _timer.addToIteration();

    ldlt.compute(batchCovariance);

    _timer.addToEigen();

    for(ifn = 0; ifn < _nFunctions; ifn++ ) {

        fbar = 0.0;
        for(i = 0; i < _npts; i++ ){
            fbar += _function[i][ifn];
        }
        fbar = fbar/T(_npts);
        _timer.addToIteration();

        for(i = 0; i < _npts; i++ ){
            batchbb(i,0) = _function[i][ifn] - fbar;
        }
        batchxx = ldlt.solve(batchbb);
        _timer.addToEigen();


        for(ii = 0; ii < nQueries; ii++ ){
            for(i = 0; i < _dimensions; i++ ) v1[i] = queries[ii][i];
            if(_useMaxMin == 1) {
                for(i = 0; i < _dimensions; i++ ) v1[i] = (v1[i] - _min[i])/(_max[i] - _min[i]);
            }
            mu[ii][ifn] = fbar;
            for(i = 0; i < _npts; i++ ){
                mu[ii][ifn] += batchxx(i)*(*_covariogram)(v1, _kdTree.getData(i));
            }
        }

    }//ifn = 0 to _nFunctions

    _timer.addToIteration();
    for(ii = 0; ii < nQueries; ii++ ){
        for(i = 0; i < _dimensions; i++ ) v1[i] = queries[ii][i];
        if(_useMaxMin == 1){
            for(i = 0; i < _dimensions; i++ ) v1[i] = (v1[i] - _min[i])/(_max[i] - _min[i]);
        }

        for(i = 0;i < _npts;i++ ) {
            batchbb(i,0) = (*_covariogram)(v1,_kdTree.getData(i));
            queryCovariance(i,0) = batchbb(i,0);
        }
        batchxx = ldlt.solve(batchbb);

        variance[ii][0] = (*_covariogram)(v1, v1) + _lambda;

        for(i = 0; i < _npts; i++ ) {
              variance[ii][0] -= queryCovariance(i, 0)*batchxx(i);
        }

        variance[ii][0] = variance[ii][0]*_krigingParameter;
        for(i = 1; i < _nFunctions; i++ ) variance[ii][i] = variance[ii][0];

    }
    _timer.addToVariance();
    _timer.addToTotal(nQueries);
}

template < typename T >
void GaussianProcess < T > ::batchInterpolate(ndarray::Array < T,1,1 >  mu,
                                              ndarray::Array < T,2,2 >  const &queries) const
{

    int i,j,ii,nQueries;

    nQueries = queries.template getSize < 0 > ();

    if(_nFunctions != 1){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your output array does not have enough room for all of the functions "
                          "you are trying to interpolate.\n");
    }

    if(queries.template getSize <1> () != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "The points at which you are trying to interpolate your function are "
                          "of the wrong dimensionality.\n");
    }

    if(mu.getNumElements() != nQueries){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your output array does not have enough room for all of the points "
                          "at which you are trying to interpolate your function.\n");
    }

    T fbar;
    Eigen::Matrix  < T,Eigen::Dynamic,Eigen::Dynamic >  batchCovariance,batchbb,batchxx;
    Eigen::Matrix  < T,Eigen::Dynamic,Eigen::Dynamic >  queryCovariance;
    Eigen::LDLT < Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >   >  ldlt;

    ndarray::Array < T,1,1 >  v1;

    _timer.start();

    v1 = allocate(ndarray::makeVector(_dimensions));

    batchbb.resize(_npts, 1);
    batchxx.resize(_npts, 1);
    batchCovariance.resize(_npts, _npts);
    queryCovariance.resize(_npts, 1);


    for(i = 0; i < _npts; i++ ) {
        batchCovariance(i, i) = (*_covariogram)(_kdTree.getData(i), _kdTree.getData(i)) + _lambda;
        for(j = i + 1; j < _npts; j++ ) {
            batchCovariance(i, j) = (*_covariogram)(_kdTree.getData(i), _kdTree.getData(j));
            batchCovariance(j, i) = batchCovariance(i, j);
        }
    }
    _timer.addToIteration();

    ldlt.compute(batchCovariance);

    fbar = 0.0;
    for(i = 0; i < _npts; i++ ) {
        fbar += _function[i][0];
    }
    fbar = fbar/T(_npts);

    for(i = 0; i < _npts; i++ ) {
        batchbb(i, 0) = _function[i][0] - fbar;
    }
    batchxx = ldlt.solve(batchbb);
    _timer.addToEigen();

    for(ii = 0; ii < nQueries; ii++ ) {

        for(i = 0; i < _dimensions; i++ ) v1[i] = queries[ii][i];
        if(_useMaxMin == 1) {
            for(i = 0; i < _dimensions; i++ )v1[i] = (v1[i] - _min[i])/(_max[i] - _min[i]);
        }

        mu(ii) = fbar;
        for(i = 0; i < _npts; i++ ) {
            mu(ii) += batchxx(i)*(*_covariogram)(v1, _kdTree.getData(i));
        }
    }
    _timer.addToIteration();
    _timer.addToTotal(nQueries);

}

template < typename T >
void GaussianProcess < T > ::batchInterpolate(ndarray::Array < T,2,2 >  mu,
                                              ndarray::Array < T,2,2 >  const &queries) const
{

    int i,j,ii,nQueries,ifn;

    nQueries = queries.template getSize < 0 > ();

    if(mu.template getSize < 0 > () != nQueries){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your output array does not have enough room for all of the points "
                          "at which you want to interpolate your functions.\n");
    }

    if(mu.template getSize < 1 > () != _nFunctions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "Your output array does not have enough room for all of the functions "
                          "you are trying to interpolate.\n");
    }

    if(queries.template getSize < 1 > () != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "The points at which you are interpolating your functions do not "
                          "have the correct dimensionality.\n");
    }

    T fbar;
    Eigen::Matrix  < T,Eigen::Dynamic,Eigen::Dynamic >  batchCovariance,batchbb,batchxx;
    Eigen::Matrix  < T,Eigen::Dynamic,Eigen::Dynamic >  queryCovariance;
    Eigen::LDLT < Eigen::Matrix < T,Eigen::Dynamic,Eigen::Dynamic >   >  ldlt;

    ndarray::Array < T,1,1 >  v1;

    _timer.start();

    v1 = allocate(ndarray::makeVector(_dimensions));

    batchbb.resize(_npts, 1);
    batchxx.resize(_npts, 1);
    batchCovariance.resize(_npts, _npts);
    queryCovariance.resize(_npts, 1);


    for(i = 0; i < _npts; i++ ){
        batchCovariance(i, i) = (*_covariogram)(_kdTree.getData(i), _kdTree.getData(i)) + _lambda;
        for(j = i + 1; j < _npts; j++ ){
            batchCovariance(i, j) = (*_covariogram)(_kdTree.getData(i), _kdTree.getData(j));
            batchCovariance(j, i) = batchCovariance(i, j);
        }
    }

    _timer.addToIteration();

    ldlt.compute(batchCovariance);

    _timer.addToEigen();

    for(ifn = 0; ifn < _nFunctions; ifn++ ){

        fbar = 0.0;
        for(i = 0; i < _npts; i++ ){
            fbar += _function[i][ifn];
        }
        fbar = fbar/T(_npts);

        _timer.addToIteration();

        for(i = 0; i < _npts; i++ ){
            batchbb(i, 0) = _function[i][ifn] - fbar;
        }
        batchxx = ldlt.solve(batchbb);
        _timer.addToEigen();


        for(ii = 0; ii < nQueries; ii++ ){
            for(i = 0; i < _dimensions; i++ )v1[i] = queries[ii][i];
            if(_useMaxMin == 1){
                for(i = 0;i < _dimensions;i++ )v1[i] = (v1[i] - _min[i])/(_max[i] - _min[i]);
            }

            mu[ii][ifn] = fbar;
            for(i = 0; i < _npts; i++ ){
                 mu[ii][ifn] += batchxx(i)*(*_covariogram)(v1, _kdTree.getData(i));
            }
        }


    }//ifn = 0 through _nFunctions

    _timer.addToTotal(nQueries);

}


template  < typename T >
void GaussianProcess < T > ::addPoint(ndarray::Array < T,1,1 >  const &vin, T f)
{

    int i,j;

    if(_nFunctions!= 1){

        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
        "You are calling the wrong addPoint; you need a vector of functions\n");

    }

    if(vin.getNumElements() != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You are trying to add a point of the wrong dimensionality to "
                          "your GaussianProcess.\n");
    }

    ndarray::Array < T,1,1 >  v;
    v = allocate(ndarray::makeVector(_dimensions));

    for(i = 0; i < _dimensions; i++ ){
        v[i] = vin[i];
        if(_useMaxMin == 1){
            v[i] = (v[i] - _min[i])/(_max[i] - _min[i]);
        }
    }

    if(_npts == _room){
        ndarray::Array < T,2,2 >  buff;
        buff = allocate(ndarray::makeVector(_npts, _nFunctions));
        buff.deep() = _function;

        _room += _roomStep;
        _function = allocate(ndarray::makeVector(_room, _nFunctions));
        for(i = 0; i < _npts; i++ ) {

            for(j = 0; j < _nFunctions; j++ ) {
                _function[i][j] = buff[i][j];
            }

        }
    }

    _function[_npts][0] = f;

    _kdTree.addPoint(v);
    _npts = _kdTree.getNPoints();

}

template  < typename T >
void GaussianProcess < T > ::addPoint(ndarray::Array < T,1,1 >  const &vin,
                                      ndarray::Array < T,1,1 >  const &f)
{

    if(vin.getNumElements() != _dimensions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You are trying to add a point of the wrong dimensionality to "
                          "your GaussianProcess.\n");
    }

    if(f.template getSize < 0 > () != _nFunctions){
        throw LSST_EXCEPT(lsst::pex::exceptions::RuntimeError,
                          "You are not adding the correct number of function values to "
                          "your GaussianProcess.\n");
    }

    int i,j;

    ndarray::Array < T,1,1 >  v;
    v = allocate(ndarray::makeVector(_dimensions));

    for(i = 0; i < _dimensions; i++ ) {
        v[i] = vin[i];
        if(_useMaxMin == 1) {
            v[i] = (v[i] - _min[i])/(_max[i] - _min[i]);
        }
    }

    if(_npts == _room) {
        ndarray::Array < T,2,2 >  buff;
        buff = allocate(ndarray::makeVector(_npts, _nFunctions));
        buff.deep() = _function;

        _room += _roomStep;
        _function = allocate(ndarray::makeVector(_room, _nFunctions));
        for(i = 0; i < _npts; i++ ) {
            for(j = 0; j < _nFunctions; j++ ) {
                 _function[i][j] = buff[i][j];
            }
        }

    }
    for(i = 0; i < _nFunctions; i++ )_function[_npts][i] = f[i];

    _kdTree.addPoint(v);
    _npts = _kdTree.getNPoints();


}

template  < typename T >
void GaussianProcess < T > ::removePoint(int dex)
{

    int i,j;

    _kdTree.removePoint(dex);

    for(i = dex; i < _npts; i++ ) {
        for(j = 0; j < _nFunctions; j++ ) {
             _function[i][j] = _function[i + 1][j];
        }
    }
    _npts = _kdTree.getNPoints();
}

template  < typename T >
void GaussianProcess < T > ::setKrigingParameter(T kk)
{
    _krigingParameter = kk;
}

template  < typename T >
void GaussianProcess < T > ::setCovariogram(std::shared_ptr <  Covariogram < T >   >  const &covar){
     _covariogram = covar;
}

template  < typename T >
void GaussianProcess < T > ::setLambda(T lambda){
    _lambda = lambda;
}


template  < typename T >
GaussianProcessTimer& GaussianProcess < T > ::getTimes() const
{
    return _timer;
}

template  < typename T >
Covariogram < T > ::~Covariogram(){};

template  < typename T >
T Covariogram < T > ::operator()(ndarray::Array < const T,1,1 >  const &p1,
                                 ndarray::Array < const T,1,1 >  const &p2) const
{
    std::cout << "by the way, you are calling the wrong operator\n";
    exit(1);
    return T(1.0);
}

template  < typename T >
SquaredExpCovariogram < T > ::~SquaredExpCovariogram(){}

template  < typename T >
SquaredExpCovariogram < T > ::SquaredExpCovariogram()
{
    _ellSquared = 1.0;
}

template  < typename T >
void SquaredExpCovariogram < T > ::setEllSquared(double ellSquared)
{
    _ellSquared = ellSquared;
}

template  < typename T >
T SquaredExpCovariogram < T > ::operator()(ndarray::Array < const T,1,1 >  const &p1,
                                           ndarray::Array < const T,1,1 >  const &p2) const
{
    int i;
    T d;
    d = 0.0;
    for(i = 0; i < p1.template getSize < 0 > (); i++ ){
        d += (p1[i] - p2[i])*(p1[i] - p2[i]);
    }

    d = d/_ellSquared;
    return T(exp( - 0.5*d));
}

template  < typename T >
NeuralNetCovariogram < T > ::~NeuralNetCovariogram(){}

template  < typename T >
NeuralNetCovariogram < T > ::NeuralNetCovariogram(){

    _sigma0 = 1.0;
    _sigma1 = 1.0;
}

template  < typename T >
T NeuralNetCovariogram < T > ::operator()(ndarray::Array < const T,1,1 >  const &p1,
                                          ndarray::Array < const T,1,1 >  const &p2
                                          ) const
{
    int i,dim;
    double num,denom1,denom2,arg;

    dim = p1.template getSize < 0 > ();

    num = 2.0*_sigma0;
    denom1 = 1.0 + 2.0*_sigma0;
    denom2 = 1.0 + 2.0*_sigma0;

    for(i = 0; i < dim; i++ ) {
        num += 2.0*p1[i]*p2[i]*_sigma1;
        denom1 += 2.0*p1[i]*p1[i]*_sigma1;
        denom2 += 2.0*p2[i]*p2[i]*_sigma1;
    }

    arg = num/::sqrt(denom1*denom2);
    return T(2.0*(::asin(arg))/3.141592654);

}

template  < typename T >
void NeuralNetCovariogram < T > ::setSigma0(double sigma0)
{
    _sigma0 = sigma0;
}

template  < typename T >
void NeuralNetCovariogram < T > ::setSigma1(double sigma1)
{
    _sigma1 = sigma1;
}

}}}

#define gpn lsst::afw::math

#define INSTANTIATEGP(T) \
        template class gpn::KdTree < T > ; \
        template class gpn::GaussianProcess < T > ; \
        template class gpn::Covariogram < T > ; \
        template class gpn::SquaredExpCovariogram < T > ;\
        template class gpn::NeuralNetCovariogram < T > ;

INSTANTIATEGP(double);