mv_pdf.c

Go to the documentation of this file.

/*
 * This work is licensed under a Creative Commons 
 * Attribution-Noncommercial-Share Alike 3.0 United States License.
 * 
 *    http://creativecommons.org/licenses/by-nc-sa/3.0/us/
 * 
 * You are free:
 * 
 *    to Share - to copy, distribute, display, and perform the work
 *    to Remix - to make derivative works
 * 
 * Under the following conditions:
 * 
 *    Attribution. You must attribute the work in the manner specified by the
 *    author or licensor (but not in any way that suggests that they endorse you
 *    or your use of the work).
 * 
 *    Noncommercial. You may not use this work for commercial purposes.
 * 
 *    Share Alike. If you alter, transform, or build upon this work, you may
 *    distribute the resulting work only under the same or similar license to
 *    this one.
 * 
 * For any reuse or distribution, you must make clear to others the license
 * terms of this work. The best way to do this is by including this header.
 * 
 * Any of the above conditions can be waived if you get permission from the
 * copyright holder.
 * 
 * Apart from the remix rights granted under this license, nothing in this
 * license impairs or restricts the author's moral rights.
 */


#include <jwsc/config.h>

#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <math.h>
#include <inttypes.h>

#include "jwsc/base/error.h"
#include "jwsc/math/constants.h"
#include "jwsc/math/lapack.h"
#include "jwsc/vector/vector.h"
#include "jwsc/vector/vector_math.h"
#include "jwsc/matrix/matrix.h"
#include "jwsc/matrix/matrix_math.h"
#include "jwsc/prob/pdf.h"
#include "jwsc/prob/mv_pdf.h"


#if defined JWSC_HAVE_LAPACK

Error* mv_gaussian_pdf_f
(
    float*          p_out,
    const Vector_f* mu, 
    const Matrix_f* sigma, 
    const Vector_f* x
)
{
    uint32_t    D;
    float       sigma_det;
    float       d;
    double      c;
    Matrix_f*   sigma_lu = NULL;
    Matrix_f*   sigma_inv = NULL;
    Vector_i32* pivot = NULL;
    Vector_f*   v_1 = NULL;
    Vector_f*   v_2 = NULL;
    Error*      e;

    D = mu->num_elts;

    if (D != sigma->num_rows || D != sigma->num_cols || D != x->num_elts)
    {
        return JWSC_EARG("Invalid argument dimensions");
    }

    if ((e = lu_decompose_matrix_f(&sigma_lu, &pivot, sigma)) != NULL)
    {
        free_error(e);
        return JWSC_EARG("Covariance matrix is singular");
    }

    calc_lu_matrix_determinant_f(&sigma_det, sigma_lu, pivot);

    if (sigma_det < 1.0e-10f)
    {
        fprintf(stderr, "%e\n", sigma_det);
        free_vector_i32(pivot);
        free_matrix_f(sigma_lu);
        return JWSC_EARG("Invalid covariance matrix");
    }

    if ((e = invert_lu_matrix_f(&sigma_inv, sigma_lu, pivot)) != NULL)
    {
        free_error(e);
        free_vector_i32(pivot);
        free_matrix_f(sigma_lu);
        return JWSC_EARG("Covariance matrix is singular");
    }

    c = sqrt(pow(JWSC_2_PI, D) * sigma_det);

    subtract_vectors_f(&v_1, x, mu);
    multiply_matrix_and_vector_f(&v_2, sigma_inv, v_1);
    calc_vector_dot_prod_f(&d, v_1, v_2);

    *p_out = (float)(exp(-0.5 * d) / c);

    free_vector_i32(pivot);
    free_matrix_f(sigma_lu);
    free_matrix_f(sigma_inv);
    free_vector_f(v_1);
    free_vector_f(v_2);

    return NULL;
}

Error* mv_gaussian_pdf_d
(
    double*         p_out,
    const Vector_d* mu, 
    const Matrix_d* sigma, 
    const Vector_d* x
)
{
    uint32_t    D;
    double      sigma_det;
    double      c, d;
    Matrix_d*   sigma_lu = NULL;
    Matrix_d*   sigma_inv = NULL;
    Vector_i32* pivot = NULL;
    Vector_d*   v_1 = NULL;
    Vector_d*   v_2 = NULL;
    Error*      e;

    D = mu->num_elts;

    if (D != sigma->num_rows || D != sigma->num_cols || D != x->num_elts)
    {
        return JWSC_EARG("Invalid argument dimensions");
    }

    if ((e = lu_decompose_matrix_d(&sigma_lu, &pivot, sigma)) != NULL)
    {
        free_error(e);
        return JWSC_EARG("Covariance matrix is singular");
    }

    calc_lu_matrix_determinant_d(&sigma_det, sigma_lu, pivot);

    if (sigma_det < 1.0e-64)
    {
        fprintf(stderr, "%e\n", sigma_det);
        free_vector_i32(pivot);
        free_matrix_d(sigma_lu);
        return JWSC_EARG("Invalid covariance matrix");
    }

    if ((e = invert_lu_matrix_d(&sigma_inv, sigma_lu, pivot)) != NULL)
    {
        free_error(e);
        free_vector_i32(pivot);
        free_matrix_d(sigma_lu);
        return JWSC_EARG("Covariance matrix is singular");
    }

    c = sqrt(pow(JWSC_2_PI, D) * sigma_det);

    subtract_vectors_d(&v_1, x, mu);
    multiply_matrix_and_vector_d(&v_2, sigma_inv, v_1);
    calc_vector_dot_prod_d(&d, v_1, v_2);

    *p_out = exp(-0.5 * d) / c;

    free_vector_i32(pivot);
    free_matrix_d(sigma_lu);
    free_matrix_d(sigma_inv);
    free_vector_d(v_1);
    free_vector_d(v_2);

    return NULL;
}

#endif


#if defined JWSC_HAVE_LAPACK

Error* set_mv_gaussian_pdf_f
(
    Vector_f**      p_out,
    const Vector_f* mu, 
    const Matrix_f* sigma, 
    const Matrix_f* x
)
{
    uint32_t    D;
    uint32_t    n, N;
    float       sigma_det;
    double      c;
    float       d;
    Matrix_f*   sigma_lu = NULL;
    Matrix_f*   sigma_inv = NULL;
    Vector_i32* pivot = NULL;
    Vector_f*   v_1 = NULL;
    Vector_f*   v_2 = NULL;
    Vector_f*   x_n = NULL;
    Vector_f*   p;
    Error*      e;

    D = x->num_cols;
    N = x->num_rows;

    if (D != sigma->num_rows || D != sigma->num_cols || D != mu->num_elts)
    {
        return JWSC_EARG("Invalid argument dimensions");
    }

    if ((e = lu_decompose_matrix_f(&sigma_lu, &pivot, sigma)) != NULL)
    {
        free_error(e);
        return JWSC_EARG("Covariance matrix is singular");
    }

    calc_lu_matrix_determinant_f(&sigma_det, sigma_lu, pivot);

    if (sigma_det < 1.0e-64)
    {
        fprintf(stderr, "%e\n", sigma_det);
        free_vector_i32(pivot);
        free_matrix_f(sigma_lu);
        return JWSC_EARG("Invalid covariance matrix");
    }

    if ((e = invert_lu_matrix_f(&sigma_inv, sigma_lu, pivot)) != NULL)
    {
        free_error(e);
        free_vector_i32(pivot);
        free_matrix_f(sigma_lu);
        return JWSC_EARG("Covariance matrix is singular");
    }

    create_vector_f(p_out, N);
    p = *p_out;

    c = sqrt(pow(JWSC_2_PI, D) * sigma_det);

    for (n = 0; n < N; n++)
    {
        copy_vector_from_matrix_f(&x_n, x, MATRIX_ROW_VECTOR, n);
        subtract_vectors_f(&v_1, x_n, mu);
        multiply_matrix_and_vector_f(&v_2, sigma_inv, v_1);
        calc_vector_dot_prod_f(&d, v_1, v_2);

        p->elts[ n ] = exp(-0.5 * d) / c;
    }

    free_vector_i32(pivot);
    free_matrix_f(sigma_lu);
    free_matrix_f(sigma_inv);
    free_vector_f(v_1);
    free_vector_f(v_2);
    free_vector_f(x_n);

    return NULL;
}

Error* set_mv_gaussian_pdf_d
(
    Vector_d**      p_out,
    const Vector_d* mu, 
    const Matrix_d* sigma, 
    const Matrix_d* x
)
{
    uint32_t    D;
    uint32_t    n, N;
    double      sigma_det;
    double      c, d;
    Matrix_d*   sigma_lu = NULL;
    Matrix_d*   sigma_inv = NULL;
    Vector_i32* pivot = NULL;
    Vector_d*   v_1 = NULL;
    Vector_d*   v_2 = NULL;
    Vector_d*   x_n = NULL;
    Vector_d*   p;
    Error*      e;

    D = x->num_cols;
    N = x->num_rows;

    if (D != sigma->num_rows || D != sigma->num_cols || D != mu->num_elts)
    {
        return JWSC_EARG("Invalid argument dimensions");
    }

    if ((e = lu_decompose_matrix_d(&sigma_lu, &pivot, sigma)) != NULL)
    {
        free_error(e);
        return JWSC_EARG("Covariance matrix is singular");
    }

    calc_lu_matrix_determinant_d(&sigma_det, sigma_lu, pivot);

    if (sigma_det < 1.0e-64)
    {
        fprintf(stderr, "%e\n", sigma_det);
        free_vector_i32(pivot);
        free_matrix_d(sigma_lu);
        return JWSC_EARG("Invalid covariance matrix");
    }

    if ((e = invert_lu_matrix_d(&sigma_inv, sigma_lu, pivot)) != NULL)
    {
        free_error(e);
        free_vector_i32(pivot);
        free_matrix_d(sigma_lu);
        return JWSC_EARG("Covariance matrix is singular");
    }

    create_vector_d(p_out, N);
    p = *p_out;

    c = sqrt(pow(JWSC_2_PI, D) * sigma_det);

    for (n = 0; n < N; n++)
    {
        copy_vector_from_matrix_d(&x_n, x, MATRIX_ROW_VECTOR, n);
        subtract_vectors_d(&v_1, x_n, mu);
        multiply_matrix_and_vector_d(&v_2, sigma_inv, v_1);
        calc_vector_dot_prod_d(&d, v_1, v_2);

        p->elts[ n ] = exp(-0.5 * d) / c;
    }

    free_vector_i32(pivot);
    free_matrix_d(sigma_lu);
    free_matrix_d(sigma_inv);
    free_vector_d(v_1);
    free_vector_d(v_2);
    free_vector_d(x_n);

    return NULL;
}

#endif


#if defined JWSC_HAVE_LAPACK

Error* log_mv_gaussian_pdf_f
(
    float*          p_out,
    const Vector_f* mu, 
    const Matrix_f* sigma, 
    const Vector_f* x
)
{
    uint32_t    D;
    float       sigma_det;
    float       d;
    double      c;
    Matrix_f*   sigma_lu = NULL;
    Matrix_f*   sigma_inv = NULL;
    Vector_i32* pivot = NULL;
    Vector_f*   v_1 = NULL;
    Vector_f*   v_2 = NULL;
    Error*      e;

    D = mu->num_elts;

    if (D != sigma->num_rows || D != sigma->num_cols || D != x->num_elts)
    {
        return JWSC_EARG("Invalid argument dimensions");
    }

    if ((e = lu_decompose_matrix_f(&sigma_lu, &pivot, sigma)) != NULL)
    {
        free_error(e);
        return JWSC_EARG("Covariance matrix is singular");
    }

    calc_lu_matrix_determinant_f(&sigma_det, sigma_lu, pivot);

    if (sigma_det < 1.0e-10f)
    {
        fprintf(stderr, "%e\n", sigma_det);
        free_vector_i32(pivot);
        free_matrix_f(sigma_lu);
        return JWSC_EARG("Invalid covariance matrix");
    }

    if ((e = invert_lu_matrix_f(&sigma_inv, sigma_lu, pivot)) != NULL)
    {
        free_error(e);
        free_vector_i32(pivot);
        free_matrix_f(sigma_lu);
        return JWSC_EARG("Covariance matrix is singular");
    }

    c = sqrt(pow(JWSC_2_PI, D) * sigma_det);

    subtract_vectors_f(&v_1, x, mu);
    multiply_matrix_and_vector_f(&v_2, sigma_inv, v_1);
    calc_vector_dot_prod_f(&d, v_1, v_2);

    *p_out = (float)(-0.5 * d - log(c));

    free_vector_i32(pivot);
    free_matrix_f(sigma_lu);
    free_matrix_f(sigma_inv);
    free_vector_f(v_1);
    free_vector_f(v_2);

    return NULL;
}

Error* log_mv_gaussian_pdf_d
(
    double*         p_out,
    const Vector_d* mu, 
    const Matrix_d* sigma, 
    const Vector_d* x
)
{
    uint32_t    D;
    double      sigma_det;
    double      c, d;
    Matrix_d*   sigma_lu = NULL;
    Matrix_d*   sigma_inv = NULL;
    Vector_i32* pivot = NULL;
    Vector_d*   v_1 = NULL;
    Vector_d*   v_2 = NULL;
    Error*      e;

    D = mu->num_elts;

    if (D != sigma->num_rows || D != sigma->num_cols || D != x->num_elts)
    {
        return JWSC_EARG("Invalid argument dimensions");
    }

    if ((e = lu_decompose_matrix_d(&sigma_lu, &pivot, sigma)) != NULL)
    {
        free_error(e);
        return JWSC_EARG("Covariance matrix is singular");
    }

    calc_lu_matrix_determinant_d(&sigma_det, sigma_lu, pivot);

    if (sigma_det < 1.0e-64)
    {
        fprintf(stderr, "%e\n", sigma_det);
        free_vector_i32(pivot);
        free_matrix_d(sigma_lu);
        return JWSC_EARG("Invalid covariance matrix");
    }

    if ((e = invert_lu_matrix_d(&sigma_inv, sigma_lu, pivot)) != NULL)
    {
        free_error(e);
        free_vector_i32(pivot);
        free_matrix_d(sigma_lu);
        return JWSC_EARG("Covariance matrix is singular");
    }

    c = sqrt(pow(JWSC_2_PI, D) * sigma_det);

    subtract_vectors_d(&v_1, x, mu);
    multiply_matrix_and_vector_d(&v_2, sigma_inv, v_1);
    calc_vector_dot_prod_d(&d, v_1, v_2);

    *p_out = -0.5 * d - log(c);

    free_vector_i32(pivot);
    free_matrix_d(sigma_lu);
    free_matrix_d(sigma_inv);
    free_vector_d(v_1);
    free_vector_d(v_2);

    return NULL;
}

#endif


void sample_standard_mv_gaussian_pdf_f
(
    Vector_f** x_out, 
    uint32_t   D
)
{
    uint32_t  d;
    uint8_t   accept;
    float     s, l, u;
    Vector_f* x;

    create_vector_f(x_out, D);
    x = *x_out;

    for (d = 0; d < D; d++)
    {
        accept = 0;

        while (!accept)
        {
            s = sample_uniform_pdf_f(-6.0, 6.0);
            l = standard_gaussian_pdf_f(s);
            u = sample_uniform_pdf_f(0, 1);

            if (u <= l)
            {
                accept = 1;
            }
        }

        x->elts[ d ] = s;
    }
}

void sample_standard_mv_gaussian_pdf_d
(
    Vector_d** x_out, 
    uint32_t   D
)
{
    uint32_t  d;
    uint8_t   accept;
    double    s, l, u;
    Vector_d* x;

    create_vector_d(x_out, D);
    x = *x_out;

    for (d = 0; d < D; d++)
    {
        accept = 0;

        while (!accept)
        {
            s = sample_uniform_pdf_d(-6.0, 6.0);
            l = standard_gaussian_pdf_d(s);
            u = sample_uniform_pdf_d(0, 1);

            if (u <= l)
            {
                accept = 1;
            }
        }

        x->elts[ d ] = s;
    }
}

void set_sample_standard_mv_gaussian_pdf_f
(
    Matrix_f** x_out, 
    uint32_t   N,
    uint32_t   D
)
{
    uint32_t  n, d;
    uint8_t   accept;
    float     s, l, u;
    Matrix_f* x;

    create_matrix_f(x_out, N, D);
    x = *x_out;

    for (n = 0; n < N; n++)
    {
        for (d = 0; d < D; d++)
        {
            accept = 0;

            while (!accept)
            {
                s = sample_uniform_pdf_f(-6.0f, 6.0f);
                l = standard_gaussian_pdf_f(s);
                u = sample_uniform_pdf_f(0, 1);

                if (u <= l)
                {
                    accept = 1;
                }
            }

            x->elts[ n ][ d ] = s;
        }
    }
}

void set_sample_standard_mv_gaussian_pdf_d
(
    Matrix_d** x_out, 
    uint32_t   N,
    uint32_t   D
)
{
    uint32_t  n, d;
    uint8_t   accept;
    double    s, l, u;
    Matrix_d* x;

    create_matrix_d(x_out, N, D);
    x = *x_out;

    for (n = 0; n < N; n++)
    {
        for (d = 0; d < D; d++)
        {
            accept = 0;

            while (!accept)
            {
                s = sample_uniform_pdf_d(-6.0, 6.0);
                l = standard_gaussian_pdf_d(s);
                u = sample_uniform_pdf_d(0, 1);

                if (u <= l)
                {
                    accept = 1;
                }
            }

            x->elts[ n ][ d ] = s;
        }
    }
}

#if defined JWSC_HAVE_LAPACK

Error* sample_mv_gaussian_pdf_f
(
    Vector_f**      x_out,
    const Vector_f* mu, 
    const Matrix_f* sigma
)
{
    uint32_t  D, d;
    Vector_f* x;
    Matrix_f* eigvec = NULL;
    Vector_f* eigval = NULL;
    Error*    err;

    D = mu->num_elts;

    sample_standard_mv_gaussian_pdf_f(x_out, D);
    x = *x_out;

    if ((err = solve_real_symmetric_matrix_eigenproblem_f(&eigvec, &eigval, 
                    sigma)))
    {
        free_vector_f(*x_out); *x_out = NULL;
        return err;
    }

    for (d = 0; d < D; d++)
    {
        if (eigval->elts[ d ] < 0)
        {
            free_vector_f(*x_out); *x_out = NULL;
            free_matrix_f(eigvec);
            free_vector_f(eigval);
            return JWSC_EARG("Covariance matrix not positive semi-definite");
        }

        x->elts[ d ] *= sqrt(eigval->elts[ d ]);
    }

    assert(multiply_matrix_and_vector_f(&x, eigvec, x) == NULL);
    assert(add_vectors_f(&x, mu, x) == NULL);

    free_matrix_f(eigvec);
    free_vector_f(eigval);

    return NULL;
}

Error* sample_mv_gaussian_pdf_d
(
    Vector_d**      x_out,
    const Vector_d* mu, 
    const Matrix_d* sigma
)
{
    uint32_t  D, d;
    Vector_d* x;
    Matrix_d* eigvec = NULL;
    Vector_d* eigval = NULL;
    Error*    err;

    D = mu->num_elts;

    sample_standard_mv_gaussian_pdf_d(x_out, D);
    x = *x_out;

    if ((err = solve_real_symmetric_matrix_eigenproblem_d(&eigvec, &eigval, 
                    sigma)))
    {
        free_vector_d(*x_out); *x_out = NULL;
        return err;
    }

    for (d = 0; d < D; d++)
    {
        if (eigval->elts[ d ] < -1.0e-16)
        {
            free_vector_d(*x_out); *x_out = NULL;
            free_matrix_d(eigvec);
            free_vector_d(eigval);
            return JWSC_EARG("Covariance matrix not positive semi-definite");
        }
        else if (eigval->elts[ d ] < 0)
        {
            eigval->elts[ d ] = 0;
        }
        else
        {
            x->elts[ d ] *= sqrt(eigval->elts[ d ]);
        }
    }

    assert(multiply_matrix_and_vector_d(&x, eigvec, x) == NULL);
    assert(add_vectors_d(&x, mu, x) == NULL);

    free_matrix_d(eigvec);
    free_vector_d(eigval);

    return NULL;
}

#endif


#if defined JWSC_HAVE_LAPACK

Error* set_sample_mv_gaussian_pdf_f
(
    Matrix_f**      x_out,
    uint32_t        N,
    const Vector_f* mu, 
    const Matrix_f* sigma
)
{
    uint32_t  D, d;
    uint32_t  n;
    Matrix_f* x;
    Matrix_f* eigvec = NULL;
    Vector_f* eigval = NULL;
    Error*    err;

    D = mu->num_elts;

    set_sample_standard_mv_gaussian_pdf_f(x_out, N, D);
    x = *x_out;

    if ((err = solve_real_symmetric_matrix_eigenproblem_f(&eigvec, &eigval, 
                    sigma)))
    {
        free_matrix_f(*x_out); *x_out = NULL;
        return err;
    }

    for (d = 0; d < D; d++)
    {
        if (eigval->elts[ d ] < 0)
        {
            free_matrix_f(*x_out); *x_out = NULL;
            free_matrix_f(eigvec);
            free_vector_f(eigval);
            return JWSC_EARG("Covariance matrix not positive semi-definite");
        }
    }

    for (n = 0; n < N; n++)
    {
        for (d = 0; d < D; d++)
        {
            x->elts[ n ][ d ] *= sqrt(eigval->elts[ d ]);
        }
    }

    transpose_matrix_f(&eigvec, eigvec);
    assert(multiply_matrices_f(&x, x, eigvec) == NULL);

    for (n = 0; n < N; n++)
    {
        for (d = 0; d < D; d++)
        {
            x->elts[ n ][ d ] += mu->elts[ d ];
        }
    }

    free_matrix_f(eigvec);
    free_vector_f(eigval);

    return NULL;
}

Error* set_sample_mv_gaussian_pdf_d
(
    Matrix_d**      x_out,
    uint32_t        N,
    const Vector_d* mu, 
    const Matrix_d* sigma
)
{
    uint32_t  D, d;
    uint32_t  n;
    Matrix_d* x;
    Matrix_d* eigvec = NULL;
    Vector_d* eigval = NULL;
    Error*    err;

    D = mu->num_elts;

    set_sample_standard_mv_gaussian_pdf_d(x_out, N, D);
    x = *x_out;

    if ((err = solve_real_symmetric_matrix_eigenproblem_d(&eigvec, &eigval, 
                    sigma)))
    {
        free_matrix_d(*x_out); *x_out = NULL;
        return err;
    }

    for (d = 0; d < D; d++)
    {
        if (eigval->elts[ d ] < 0)
        {
            free_matrix_d(*x_out); *x_out = NULL;
            free_matrix_d(eigvec);
            free_vector_d(eigval);
            return JWSC_EARG("Covariance matrix not positive semi-definite");
        }
    }

    for (n = 0; n < N; n++)
    {
        for (d = 0; d < D; d++)
        {
            x->elts[ n ][ d ] *= sqrt(eigval->elts[ d ]);
        }
    }

    transpose_matrix_d(&eigvec, eigvec);
    assert(multiply_matrices_d(&x, x, eigvec) == NULL);

    for (n = 0; n < N; n++)
    {
        for (d = 0; d < D; d++)
        {
            x->elts[ n ][ d ] += mu->elts[ d ];
        }
    }

    free_matrix_d(eigvec);
    free_vector_d(eigval);

    return NULL;
}

#endif