WGaussProcess.cpp 16.2 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43
//---------------------------------------------------------------------------
//
// Project: OpenWalnut ( http://www.openwalnut.org )
//
// Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
// For more information see http://www.openwalnut.org/copying
//
// This file is part of OpenWalnut.
//
// OpenWalnut is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// OpenWalnut 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 Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
//
//---------------------------------------------------------------------------

#include <Eigen/QR>

#include "core/common/datastructures/WFiber.h"
#include "core/common/WAssert.h"

#include "WGaussProcess.h"

WGaussProcess::WGaussProcess( const size_t tractID,
                              boost::shared_ptr< const WDataSetFibers > tracts,
                              boost::shared_ptr< const WDataSetDTI > tensors,
                              double maxLevel )
    : m_tractID( tractID ),
      m_tracts( tracts ),
      m_tensors( tensors ),
      m_maxLevel( maxLevel )
{
    WFiber tract = generateTract();
    m_Cff_1_l_product = Eigen::VectorXd( static_cast< int >( tract.size() ) );
    m_R = 2.0 * maxSegmentLength( tract );
Stefan Philips's avatar
Stefan Philips committed
44
    m_Cff_1_l_product = generateCffInverse( tract ) * Eigen::VectorXd( Eigen::VectorXd::Ones( m_Cff_1_l_product.size() ) * m_maxLevel );
45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192
    generateTauParameter();
    m_bb = computeBoundingBox( tract );
}

WGaussProcess::~WGaussProcess()
{
}

double WGaussProcess::mean( const WPosition& p ) const
{
    Eigen::VectorXd Sf( m_Cff_1_l_product.size() );

    // for further improvement we could work with the indices of the arrays inside of the WDataSetFibers instead of building up this point vector
    WFiber tract = generateTract();

    for( size_t i = 0; i < tract.size(); ++i )
    {
        Sf( i ) = cov_s( tract[i], p );
    }

    return Sf.dot( m_Cff_1_l_product );
}

WFiber WGaussProcess::generateTract() const
{
    return ( *m_tracts )[ m_tractID ];
}

Eigen::MatrixXd WGaussProcess::generateCffInverse( const WFiber& tract )
{
    Eigen::MatrixXd Cff( static_cast< int >( tract.size() ), static_cast< int >( tract.size() ) );
    size_t i = 0, j = 0;
    for( WFiber::const_iterator cit = tract.begin(); cit != tract.end(); ++cit, ++i )
    {
        for( WFiber::const_iterator cit2 = tract.begin(); cit2 != tract.end(); ++cit2, ++j )
        {
            Cff( i, j ) = cov( *cit, *cit2 );
        }
        j = 0; // reset every loop!
    }

    // Note: If Cff is constructed via a positive definite function itself is positive definite,
    // hence invertible
    Eigen::ColPivHouseholderQR< Eigen::MatrixXd > qrOfCff( Cff );
    return qrOfCff.inverse();
}

double WGaussProcess::generateTauParameter()
{
    double result = 0.0;

    // According to Demian this function is very complex, involing tensor interpolation (not
    // component wise) which is not trivial, but the out come does not contribute significantly to
    // the result, so I ommit the implementation at first.
    //
    // WTensorSym< 2, 3 > t = tensors->interpolate( *cit );
    // it may occur due to interpolation and noise that negative eigenvalues will occour!
    // double lambda_1 = 0.0; // = t.eigenvalues(
    // newTau = m_R / std::sqrt( lambda_1 );

    return result;
}

double WGaussProcess::cov_d( const WPosition& /* p1 */, const WPosition& /* p2 */ ) const
{
    // According to Demian this function is very complex, involing tensor interpolation (not
    // component wise) which is not trivial, but the out come does not contribute significantly to
    // the result, so I ommit the implementation at first.
    return 0.0;
}

const WBoundingBox& WGaussProcess::getBB() const
{
    return m_bb;
}

/**
 * This namespace is to prevent this method beeing public accessible. It should be used only inside here!
 */
namespace
{
    /**
     * This implements the integral over the intersecton of the two shperes defined by the base
     * points of the two fibers and the maximal segement length. It is generated by Maple^TM and
     * therefore not really readable.
     *
     * \param w The distance between the two base points
     * \param Q The radius of the first sphere
     * \param R The radius of the second sphere
     *
     * \return The integral as defined in eq. (19) of the Demian Wasserman paper.
     *
     * \note The source code does not comply to our coding standard since its generated and may change again and again.
     */
    double covIntegralThinPlateR3Normalized( const double w, const double Q, const double R )
    {
        if( w == 0.0e0 )
            return(0.3141592654e1 * pow(Q, 0.9e1) / 0.9e1 + (-0.9e1 / 0.35e2 * 0.3141592654e1 * pow(Q, 0.8e1) + 0.4e1 / 0.15e2 * 0.3141592654e1 * pow(Q, 0.6e1) * R * R) * R); // NOLINT line length
        else if(w <= R - Q) // NOLINT missing braces
            if(w <= Q / 0.2e1) // NOLINT spaces between brackets
                return(-0.4e1 / 0.1575e4 * 0.3141592654e1 * pow(w, 0.9e1) + (0.3141592654e1 * pow(w, 0.8e1) / 0.105e3 + (-0.4e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (0.6e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (0.4e1 / 0.7e1 * 0.3141592654e1 * w * w + 0.3141592654e1 * Q * Q / 0.9e1) * Q * Q) * Q * Q) * Q * Q) * Q + ((-0.4e1 / 0.5e1 * 0.3141592654e1 * w * w - 0.9e1 / 0.35e2 * 0.3141592654e1 * Q * Q) * pow(Q, 0.6e1) + 0.4e1 / 0.15e2 * 0.3141592654e1 * pow(Q, 0.6e1) * R * R) * R); // NOLINT line length
            else if(w < Q) // NOLINT missing braces
                return(-0.4e1 / 0.1575e4 * 0.3141592654e1 * pow(w, 0.9e1) + (0.3141592654e1 * pow(w, 0.8e1) / 0.105e3 + (-0.4e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (0.6e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (0.4e1 / 0.7e1 * 0.3141592654e1 * w * w + 0.3141592654e1 * Q * Q / 0.9e1) * Q * Q) * Q * Q) * Q * Q) * Q + ((-0.4e1 / 0.5e1 * 0.3141592654e1 * w * w - 0.9e1 / 0.35e2 * 0.3141592654e1 * Q * Q) * pow(Q, 0.6e1) + 0.4e1 / 0.15e2 * 0.3141592654e1 * pow(Q, 0.6e1) * R * R) * R); // NOLINT line length
            else if(Q <= w) // NOLINT missing braces
                return((0.8e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (0.12e2 / 0.35e2 * 0.3141592654e1 * w + 0.8e1 / 0.525e3 * 0.3141592654e1 / w * Q * Q) * Q * Q) * pow(Q, 0.6e1) + ((-0.4e1 / 0.5e1 * 0.3141592654e1 * w * w - 0.9e1 / 0.35e2 * 0.3141592654e1 * Q * Q) * pow(Q, 0.6e1) + 0.4e1 / 0.15e2 * 0.3141592654e1 * pow(Q, 0.6e1) * R * R) * R);  // NOLINT line length
            else // NOLINT missing braces
                return(0.0e0); // NOLINT spaces between brackets
        else if(R - Q < w && w <= R) // NOLINT missing braces
            if(w <= Q / 0.2e1) // NOLINT spaces between brackets
                return(-0.2e1 / 0.525e3 * 0.3141592654e1 * pow(w, 0.9e1) + (0.3141592654e1 * pow(w, 0.8e1) / 0.210e3 + (-0.2e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (0.3e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (-0.4e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (0.2e1 / 0.7e1 * 0.3141592654e1 * w * w + (-0.6e1 / 0.35e2 * 0.3141592654e1 * w + (0.3141592654e1 / 0.18e2 - 0.4e1 / 0.525e3 * 0.3141592654e1 / w * Q) * Q) * Q) * Q) * Q) * Q * Q) * Q * Q) * Q + (0.3141592654e1 * pow(w, 0.8e1) / 0.210e3 + (0.9e1 / 0.560e3 * 0.3141592654e1 * pow(w, 0.7e1) + (-0.3141592654e1 * pow(w, 0.5e1) / 0.20e2 + (0.9e1 / 0.40e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.5e1 * 0.3141592654e1 * w * w + (0.9e1 / 0.28e2 * 0.3141592654e1 * w + (-0.9e1 / 0.70e2 * 0.3141592654e1 + 0.3141592654e1 / w * Q / 0.48e2) * Q) * Q) * Q) * Q * Q) * Q * Q) * Q + (-0.2e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (-0.3141592654e1 * pow(w, 0.5e1) / 0.20e2 + (0.3141592654e1 * pow(w, 0.3e1) / 0.12e2 + (-0.3e1 / 0.20e2 * 0.3141592654e1 * w + (0.2e1 / 0.15e2 * 0.3141592654e1 - 0.3141592654e1 / w * Q / 0.28e2) * Q) * Q * Q) * Q * Q) * Q + (0.3e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (0.9e1 / 0.40e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.3e1 / 0.20e2 * 0.3141592654e1 * w + 0.9e1 / 0.200e3 * 0.3141592654e1 / w * Q * Q) * Q * Q) * Q + (-0.4e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.5e1 * 0.3141592654e1 * w * w + 0.2e1 / 0.15e2 * 0.3141592654e1 * Q * Q) * Q + (0.2e1 / 0.7e1 * 0.3141592654e1 * w * w + (0.9e1 / 0.28e2 * 0.3141592654e1 * w - 0.3141592654e1 / w * Q * Q / 0.28e2) * Q + (-0.6e1 / 0.35e2 * 0.3141592654e1 * w - 0.9e1 / 0.70e2 * 0.3141592654e1 * Q + (0.3141592654e1 / 0.18e2 + 0.3141592654e1 / w * Q / 0.48e2 - 0.4e1 / 0.525e3 * 0.3141592654e1 / w * R) * R) * R) * R) * R) * R * R) * R * R) * R); // NOLINT line length
            else if(w < Q) // NOLINT missing braces
                return(-0.2e1 / 0.525e3 * 0.3141592654e1 * pow(w, 0.9e1) + (0.3141592654e1 * pow(w, 0.8e1) / 0.210e3 + (-0.2e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (0.3e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (-0.4e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (0.2e1 / 0.7e1 * 0.3141592654e1 * w * w + (-0.6e1 / 0.35e2 * 0.3141592654e1 * w + (0.3141592654e1 / 0.18e2 - 0.4e1 / 0.525e3 * 0.3141592654e1 / w * Q) * Q) * Q) * Q) * Q) * Q * Q) * Q * Q) * Q + (0.3141592654e1 * pow(w, 0.8e1) / 0.210e3 + (0.9e1 / 0.560e3 * 0.3141592654e1 * pow(w, 0.7e1) + (-0.3141592654e1 * pow(w, 0.5e1) / 0.20e2 + (0.9e1 / 0.40e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.5e1 * 0.3141592654e1 * w * w + (0.9e1 / 0.28e2 * 0.3141592654e1 * w + (-0.9e1 / 0.70e2 * 0.3141592654e1 + 0.3141592654e1 / w * Q / 0.48e2) * Q) * Q) * Q) * Q * Q) * Q * Q) * Q + (-0.2e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (-0.3141592654e1 * pow(w, 0.5e1) / 0.20e2 + (0.3141592654e1 * pow(w, 0.3e1) / 0.12e2 + (-0.3e1 / 0.20e2 * 0.3141592654e1 * w + (0.2e1 / 0.15e2 * 0.3141592654e1 - 0.3141592654e1 / w * Q / 0.28e2) * Q) * Q * Q) * Q * Q) * Q + (0.3e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (0.9e1 / 0.40e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.3e1 / 0.20e2 * 0.3141592654e1 * w + 0.9e1 / 0.200e3 * 0.3141592654e1 / w * Q * Q) * Q * Q) * Q + (-0.4e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.5e1 * 0.3141592654e1 * w * w + 0.2e1 / 0.15e2 * 0.3141592654e1 * Q * Q) * Q + (0.2e1 / 0.7e1 * 0.3141592654e1 * w * w + (0.9e1 / 0.28e2 * 0.3141592654e1 * w - 0.3141592654e1 / w * Q * Q / 0.28e2) * Q + (-0.6e1 / 0.35e2 * 0.3141592654e1 * w - 0.9e1 / 0.70e2 * 0.3141592654e1 * Q + (0.3141592654e1 / 0.18e2 + 0.3141592654e1 / w * Q / 0.48e2 - 0.4e1 / 0.525e3 * 0.3141592654e1 / w * R) * R) * R) * R) * R) * R * R) * R * R) * R); // NOLINT line length
            else if(Q <= w) // NOLINT missing braces
                return(-0.2e1 / 0.1575e4 * 0.3141592654e1 * pow(w, 0.9e1) + (-0.3141592654e1 * pow(w, 0.8e1) / 0.210e3 + (0.2e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (-0.3e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (0.4e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.7e1 * 0.3141592654e1 * w * w + (0.6e1 / 0.35e2 * 0.3141592654e1 * w + (-0.3141592654e1 / 0.18e2 + 0.4e1 / 0.525e3 * 0.3141592654e1 / w * Q) * Q) * Q) * Q) * Q) * Q * Q) * Q * Q) * Q + (0.3141592654e1 * pow(w, 0.8e1) / 0.210e3 + (0.9e1 / 0.560e3 * 0.3141592654e1 * pow(w, 0.7e1) + (-0.3141592654e1 * pow(w, 0.5e1) / 0.20e2 + (0.9e1 / 0.40e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.5e1 * 0.3141592654e1 * w * w + (0.9e1 / 0.28e2 * 0.3141592654e1 * w + (-0.9e1 / 0.70e2 * 0.3141592654e1 + 0.3141592654e1 / w * Q / 0.48e2) * Q) * Q) * Q) * Q * Q) * Q * Q) * Q + (-0.2e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (-0.3141592654e1 * pow(w, 0.5e1) / 0.20e2 + (0.3141592654e1 * pow(w, 0.3e1) / 0.12e2 + (-0.3e1 / 0.20e2 * 0.3141592654e1 * w + (0.2e1 / 0.15e2 * 0.3141592654e1 - 0.3141592654e1 / w * Q / 0.28e2) * Q) * Q * Q) * Q * Q) * Q + (0.3e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (0.9e1 / 0.40e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.3e1 / 0.20e2 * 0.3141592654e1 * w + 0.9e1 / 0.200e3 * 0.3141592654e1 / w * Q * Q) * Q * Q) * Q + (-0.4e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.5e1 * 0.3141592654e1 * w * w + 0.2e1 / 0.15e2 * 0.3141592654e1 * Q * Q) * Q + (0.2e1 / 0.7e1 * 0.3141592654e1 * w * w + (0.9e1 / 0.28e2 * 0.3141592654e1 * w - 0.3141592654e1 / w * Q * Q / 0.28e2) * Q + (-0.6e1 / 0.35e2 * 0.3141592654e1 * w - 0.9e1 / 0.70e2 * 0.3141592654e1 * Q + (0.3141592654e1 / 0.18e2 + 0.3141592654e1 / w * Q / 0.48e2 - 0.4e1 / 0.525e3 * 0.3141592654e1 / w * R) * R) * R) * R) * R) * R * R) * R * R) * R); // NOLINT line length
            else // NOLINT missing braces
                return(0.0e0); // NOLINT spaces between brackets
        else if(R < w && w <= Q) // NOLINT missing braces
            return((-0.3e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (0.9e1 / 0.40e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.3e1 / 0.20e2 * 0.3141592654e1 * w + 0.9e1 / 0.200e3 * 0.3141592654e1 / w * Q * Q) * Q * Q) * Q + (0.4e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.5e1 * 0.3141592654e1 * w * w + 0.2e1 / 0.15e2 * 0.3141592654e1 * Q * Q) * Q + (-0.2e1 / 0.7e1 * 0.3141592654e1 * w * w + (0.9e1 / 0.28e2 * 0.3141592654e1 * w - 0.3141592654e1 / w * Q * Q / 0.28e2) * Q + (0.6e1 / 0.35e2 * 0.3141592654e1 * w - 0.9e1 / 0.70e2 * 0.3141592654e1 * Q + (-0.3141592654e1 / 0.18e2 + 0.3141592654e1 / w * Q / 0.48e2 + 0.4e1 / 0.525e3 * 0.3141592654e1 / w * R) * R) * R) * R) * R) * pow(R, 0.5e1));  // NOLINT line length
        else if(R < w && Q < w && w <= R + Q) // NOLINT missing braces
            return(0.2e1 / 0.1575e4 * 0.3141592654e1 * pow(w, 0.9e1) + (-0.3141592654e1 * pow(w, 0.8e1) / 0.210e3 + (0.2e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (-0.3e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (0.4e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.7e1 * 0.3141592654e1 * w * w + (0.6e1 / 0.35e2 * 0.3141592654e1 * w + (-0.3141592654e1 / 0.18e2 + 0.4e1 / 0.525e3 * 0.3141592654e1 / w * Q) * Q) * Q) * Q) * Q) * Q * Q) * Q * Q) * Q + (-0.3141592654e1 * pow(w, 0.8e1) / 0.210e3 + (0.9e1 / 0.560e3 * 0.3141592654e1 * pow(w, 0.7e1) + (-0.3141592654e1 * pow(w, 0.5e1) / 0.20e2 + (0.9e1 / 0.40e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.5e1 * 0.3141592654e1 * w * w + (0.9e1 / 0.28e2 * 0.3141592654e1 * w + (-0.9e1 / 0.70e2 * 0.3141592654e1 + 0.3141592654e1 / w * Q / 0.48e2) * Q) * Q) * Q) * Q * Q) * Q * Q) * Q + (0.2e1 / 0.105e3 * 0.3141592654e1 * pow(w, 0.6e1) + (-0.3141592654e1 * pow(w, 0.5e1) / 0.20e2 + (0.3141592654e1 * pow(w, 0.3e1) / 0.12e2 + (-0.3e1 / 0.20e2 * 0.3141592654e1 * w + (0.2e1 / 0.15e2 * 0.3141592654e1 - 0.3141592654e1 / w * Q / 0.28e2) * Q) * Q * Q) * Q * Q) * Q + (-0.3e1 / 0.25e2 * 0.3141592654e1 * pow(w, 0.4e1) + (0.9e1 / 0.40e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.3e1 / 0.20e2 * 0.3141592654e1 * w + 0.9e1 / 0.200e3 * 0.3141592654e1 / w * Q * Q) * Q * Q) * Q + (0.4e1 / 0.15e2 * 0.3141592654e1 * pow(w, 0.3e1) + (-0.2e1 / 0.5e1 * 0.3141592654e1 * w * w + 0.2e1 / 0.15e2 * 0.3141592654e1 * Q * Q) * Q + (-0.2e1 / 0.7e1 * 0.3141592654e1 * w * w + (0.9e1 / 0.28e2 * 0.3141592654e1 * w - 0.3141592654e1 / w * Q * Q / 0.28e2) * Q + (0.6e1 / 0.35e2 * 0.3141592654e1 * w - 0.9e1 / 0.70e2 * 0.3141592654e1 * Q + (-0.3141592654e1 / 0.18e2 + 0.3141592654e1 / w * Q / 0.48e2 + 0.4e1 / 0.525e3 * 0.3141592654e1 / w * R) * R) * R) * R) * R) * R * R) * R * R) * R); // NOLINT line length
        else // NOLINT missing braces
            return(0.0e0); // NOLINT spaces between brackets
    }
}

double gauss::innerProduct( const WGaussProcess& p1, const WGaussProcess& p2 )
{
    WFiber f1 = p1.generateTract();
    WFiber f2 = p2.generateTract();
    double Q = p1.getRadius();
    double R = p2.getRadius();

    Eigen::MatrixXd integralMatrix( static_cast< int >( f1.size() ), static_cast< int >( f2.size() ) );
    size_t i = 0, j = 0;
    for( WFiber::const_iterator cit = f1.begin(); cit != f1.end(); ++cit, ++i )
    {
        for( WFiber::const_iterator cit2 = f2.begin(); cit2 != f2.end(); ++cit2, ++j )
        {
            integralMatrix( i, j ) = covIntegralThinPlateR3Normalized( length( *cit - *cit2 ), Q, R );
        }
        j = 0; // reset every loop!
    }
    const Eigen::VectorXd left = p1.getCff1lProduct();
    const Eigen::VectorXd right = p2.getCff1lProduct();

    return left.dot( integralMatrix * right );
}