PosteriorModelBuilder.hxx

/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef __PosteriorModelBuilder_hxx
#define __PosteriorModelBuilder_hxx

#include "PosteriorModelBuilder.h"

#include <iostream>

#include <Eigen/SVD>

#include "CommonTypes.h"
#include "PCAModelBuilder.h"

namespace statismo {

//
// PosteriorModelBuilder
//
//

template <typename T>
PosteriorModelBuilder<T>::PosteriorModelBuilder()
    : Superclass() {
}


template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModel(
    const DataItemListType& sampleDataList,
    const PointValueListType& pointValues,
    double pointValuesNoiseVariance,
    double noiseVariance) const {
    return BuildNewModel(sampleDataList, TrivialPointValueWithCovarianceListWithUniformNoise(pointValues, pointValuesNoiseVariance), noiseVariance);
}


template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModelFromModel(
    const StatisticalModelType* inputModel,
    const PointValueListType& pointValues,
    double pointValuesNoiseVariance,
    bool computeScores) const {

    return BuildNewModelFromModel(inputModel, TrivialPointValueWithCovarianceListWithUniformNoise(pointValues,pointValuesNoiseVariance), computeScores);

}

template <typename T>
typename PosteriorModelBuilder<T>::PointValueWithCovarianceListType
PosteriorModelBuilder<T>::TrivialPointValueWithCovarianceListWithUniformNoise(
    const PointValueListType& pointValues, double pointValueNoiseVariance) const {

    const MatrixType pointCovarianceMatrix = pointValueNoiseVariance * MatrixType::Identity(3,3);
    PointValueWithCovarianceListType pvcList;//(pointValues.size());


    for (typename PointValueListType::const_iterator it = pointValues.begin(); it != pointValues.end(); ++it) {
        pvcList.push_back(PointValueWithCovariancePairType(*it,pointCovarianceMatrix));
    }

    return pvcList;

}


template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModel(
    const DataItemListType& sampleDataList,
    const PointValueWithCovarianceListType& pointValuesWithCovariance,
    double noiseVariance) const {
    typedef PCAModelBuilder<T> PCAModelBuilderType;
    PCAModelBuilderType* modelBuilder = PCAModelBuilderType::Create();
    StatisticalModelType* model = modelBuilder->BuildNewModel(sampleDataList, noiseVariance);
    StatisticalModelType* PosteriorModel = BuildNewModelFromModel(model, pointValuesWithCovariance, noiseVariance);
    delete modelBuilder;
    delete model;
    return PosteriorModel;
}


template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModelFromModel(
    const StatisticalModelType* inputModel,
    const PointValueWithCovarianceListType& pointValuesWithCovariance,
    bool computeScores) const {

    typedef statismo::Representer<T> RepresenterType;

    const RepresenterType* representer = inputModel->GetRepresenter();


    // The naming of the variables correspond to those used in the paper
    // Posterior Shape Models,
    // Thomas Albrecht, Marcel Luethi, Thomas Gerig, Thomas Vetter
    //
    const MatrixType& Q =  inputModel->GetPCABasisMatrix();
    const VectorType& mu = inputModel->GetMeanVector();

    // this method only makes sense for a proper PPCA model (e.g. the noise term is properly defined)
    // if the model has zero noise, we assume a small amount of noise
    double rho2 = std::max((double) inputModel->GetNoiseVariance(), (double) Superclass::TOLERANCE);

    unsigned dim = representer->GetDimensions();


    // build the part matrices with , considering only the points that are fixed
    //
    unsigned numPrincipalComponents = inputModel->GetNumberOfPrincipalComponents();
    MatrixType Q_g(pointValuesWithCovariance.size()* dim, numPrincipalComponents);
    VectorType mu_g(pointValuesWithCovariance.size() * dim);
    VectorType s_g(pointValuesWithCovariance.size() * dim);

    MatrixType LQ_g(pointValuesWithCovariance.size()* dim, numPrincipalComponents);

    unsigned i = 0;
    for (typename PointValueWithCovarianceListType::const_iterator it = pointValuesWithCovariance.begin(); it != pointValuesWithCovariance.end(); ++it) {
        VectorType val = representer->PointSampleToPointSampleVector(it->first.second);
        unsigned pt_id = representer->GetPointIdForPoint(it->first.first);

        // In the formulas, we actually need the precision matrix, which is the inverse of the covariance.
        const MatrixType pointPrecisionMatrix = it->second.inverse();

        // Get the three rows pertaining to this point:
        const MatrixType Qrows_for_pt_id = Q.block(pt_id * dim, 0, dim, numPrincipalComponents);

        Q_g.block(i * dim, 0, dim, numPrincipalComponents) = Qrows_for_pt_id;
        mu_g.block(i * dim, 0, dim, 1) = mu.block(pt_id * dim, 0, dim, 1);
        s_g.block(i * dim, 0, dim, 1) = val;

        LQ_g.block(i * dim, 0, dim, numPrincipalComponents) = pointPrecisionMatrix * Qrows_for_pt_id;
        i++;
    }

    VectorType D2 = inputModel->GetPCAVarianceVector().array();

    const MatrixType& Q_gT = Q_g.transpose();

    MatrixType M = Q_gT * LQ_g;
    M.diagonal() += VectorType::Ones(Q_g.cols());

    MatrixTypeDoublePrecision Minv = M.cast<double>().inverse();

    // the MAP solution for the latent variables (coefficients)
    VectorType coeffs = Minv.cast<ScalarType>() * LQ_g.transpose() * (s_g - mu_g);

    // the MAP solution in the sample space
    VectorType mu_c = inputModel->GetRepresenter()->SampleToSampleVector(inputModel->DrawSample(coeffs));

    const VectorType& pcaVariance = inputModel->GetPCAVarianceVector();
    VectorTypeDoublePrecision pcaSdev = pcaVariance.cast<double>().array().sqrt();

    VectorType D2MinusRho = D2 - VectorType::Ones(D2.rows()) * rho2;
    // the values of D2 can be negative. We need to be careful when taking the root
    for (unsigned i = 0; i < D2MinusRho.rows(); i++) {
        D2MinusRho(i) = std::max((ScalarType) 0, D2(i));
    }
    VectorType D2MinusRhoSqrt = D2MinusRho.array().sqrt();


    typedef Eigen::JacobiSVD<MatrixTypeDoublePrecision> SVDType;
    MatrixTypeDoublePrecision innerMatrix = D2MinusRhoSqrt.cast<double>().asDiagonal() * Minv * D2MinusRhoSqrt.cast<double>().asDiagonal();
    SVDType svd(innerMatrix, Eigen::ComputeThinU);


    // SVD of the inner matrix
    VectorType D_c = svd.singularValues().cast<ScalarType>();

    // Todo: Maybe it is possible to do this with Q, so that we don"t need to get U as well.
    MatrixType U_c = inputModel->GetOrthonormalPCABasisMatrix() * svd.matrixU().cast<ScalarType>();

    StatisticalModelType* PosteriorModel = StatisticalModelType::Create(representer , mu_c, U_c, D_c, rho2);

    // Write the parameters used to build the models into the builderInfo

    typename ModelInfo::BuilderInfoList builderInfoList = inputModel->GetModelInfo().GetBuilderInfoList();

    BuilderInfo::ParameterInfoList bi;
    bi.push_back(BuilderInfo::KeyValuePair("NoiseVariance ", Utils::toString(rho2)));
    bi.push_back(BuilderInfo::KeyValuePair("FixedPointsVariance ", Utils::toString(0.2)));
//
    BuilderInfo::DataInfoList di;

    unsigned pt_no = 0;
    for (typename PointValueWithCovarianceListType::const_iterator it = pointValuesWithCovariance.begin(); it != pointValuesWithCovariance.end(); ++it) {
        VectorType val = representer->PointSampleToPointSampleVector(it->first.second);

        // TODO we looked up the PointId for the same point before. Having it here again is inefficient.
        unsigned pt_id = representer->GetPointIdForPoint(it->first.first);
        std::ostringstream keySStream;
        keySStream << "Point constraint " << pt_no;
        std::ostringstream valueSStream;
        valueSStream << "(" << pt_id << ", (";

        for (unsigned d = 0; d < dim - 1; d++) {
            valueSStream << val[d] << ",";
        }
        valueSStream << val[dim -1];
        valueSStream << "))";
        di.push_back(BuilderInfo::KeyValuePair(keySStream.str(), valueSStream.str()));
        pt_no++;
    }


    BuilderInfo builderInfo("PosteriorModelBuilder", di, bi);
    builderInfoList.push_back(builderInfo);

    MatrixType inputScores = inputModel->GetModelInfo().GetScoresMatrix();
    MatrixType scores = MatrixType::Zero(inputScores.rows(), inputScores.cols());

    if (computeScores == true) {

        // get the scores from the input model
        for (unsigned i = 0; i < inputScores.cols(); i++) {
            // reconstruct the sample from the input model and project it back into the model
            typename RepresenterType::DatasetPointerType ds = inputModel->DrawSample(inputScores.col(i));
            scores.col(i) = PosteriorModel->ComputeCoefficientsForDataset(ds);
            representer->DeleteDataset(ds);
        }
    }
    ModelInfo info(scores, builderInfoList);
    PosteriorModel->SetModelInfo(info);

    return PosteriorModel;

}

} // namespace statismo

#endif