regress.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/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"


#if defined JWSC_HAVE_LAPACK

Error* linear_least_squares_regress_f
(
    Vector_f**      w_out,
    const Matrix_f* x,
    const Vector_f* y
)
{
    Matrix_f* x_inv = NULL;
    Error*    err;

    if ((err = pseudoinvert_matrix_f(&x_inv, x)) != NULL)
    {
        free_vector_f(*w_out); *w_out = NULL;
        return err;
    }

    assert(multiply_matrix_and_vector_f(w_out, x_inv, y) == NULL);

    free_matrix_f(x_inv);

    return NULL;
}

Error* linear_least_squares_regress_d
(
    Vector_d**      w_out,
    const Matrix_d* x,
    const Vector_d* y
)
{
    Matrix_d* x_inv = NULL;
    Error*    err;

    if ((err = pseudoinvert_matrix_d(&x_inv, x)) != NULL)
    {
        free_vector_d(*w_out); *w_out = NULL;
        return err;
    }

    assert(multiply_matrix_and_vector_d(w_out, x_inv, y) == NULL);

    free_matrix_d(x_inv);

    return NULL;
}

static void split_ransac_points_f
(
    Matrix_f**       x_1, 
    Vector_f**       y_1,
    Matrix_f**       x_2,
    Vector_f**       y_2,
    const Matrix_f*  x, 
    const Vector_f*  y,
    uint32_t         n_1
)
{
    uint32_t num_cols;
    uint32_t row, col;
    uint32_t row_1, row_2;
    uint32_t n, n_2;
    uint32_t i;
    uint32_t done;

    Vector_u32* decider = NULL;

    num_cols = x->num_cols;
    n_2 = x->num_rows - n_1;

    create_matrix_f(x_1, n_1, num_cols);
    create_vector_f(y_1, n_1);

    create_matrix_f(x_2, n_2, num_cols);
    create_vector_f(y_2, n_2);

    create_zero_vector_u32(&decider, x->num_rows);
    for (n = 0; n < n_1; n++)
    {
        done = 0;
        while (!done)
        {
            i = (uint32_t) floor(sample_uniform_pdf_f(0, 0.999*(x->num_rows)));

            if (!decider->elts[ i ])
            {
                done = decider->elts[ i ] = 1;
            }
        }
    }

    row_1 = 0;
    row_2 = 0;

    for (row = 0; row < x->num_rows; row++)
    {
        if (decider->elts[ row ])
        {
            for (col = 0; col < num_cols; col++)
            {
                (*x_1)->elts[ row_1 ][ col ] = x->elts[ row ][ col ];
            }
            (*y_1)->elts[ row_1 ] = y->elts[ row ];

            row_1++;
        }
        else
        {
            for (col = 0; col < num_cols; col++)
            {
                (*x_2)->elts[ row_2 ][ col ] = x->elts[ row ][ col ];
            }
            (*y_2)->elts[ row_2 ] = y->elts[ row ];

            row_2++;
        }
    }

    free_vector_u32(decider);
}

static void split_ransac_points_d
(
    Matrix_d**       x_1, 
    Vector_d**       y_1,
    Matrix_d**       x_2,
    Vector_d**       y_2,
    const Matrix_d*  x, 
    const Vector_d*  y,
    uint32_t         n_1
)
{
    uint32_t num_cols;
    uint32_t row, col;
    uint32_t row_1, row_2;
    uint32_t n, n_2;
    uint32_t i;
    uint32_t done;

    Vector_u32* decider = NULL;

    num_cols = x->num_cols;
    n_2 = x->num_rows - n_1;

    create_matrix_d(x_1, n_1, num_cols);
    create_vector_d(y_1, n_1);

    create_matrix_d(x_2, n_2, num_cols);
    create_vector_d(y_2, n_2);

    create_zero_vector_u32(&decider, x->num_rows);
    for (n = 0; n < n_1; n++)
    {
        done = 0;
        while (!done)
        {
            i = (uint32_t) floor(sample_uniform_pdf_d(0, 0.999*(x->num_rows)));

            if (!decider->elts[ i ])
            {
                done = decider->elts[ i ] = 1;
            }
        }
    }

    row_1 = 0;
    row_2 = 0;

    for (row = 0; row < x->num_rows; row++)
    {
        if (decider->elts[ row ])
        {
            for (col = 0; col < num_cols; col++)
            {
                (*x_1)->elts[ row_1 ][ col ] = x->elts[ row ][ col ];
            }
            (*y_1)->elts[ row_1 ] = y->elts[ row ];

            row_1++;
        }
        else
        {
            for (col = 0; col < num_cols; col++)
            {
                (*x_2)->elts[ row_2 ][ col ] = x->elts[ row ][ col ];
            }
            (*y_2)->elts[ row_2 ] = y->elts[ row ];

            row_2++;
        }
    }

    free_vector_u32(decider);
}

static void append_ransac_fit_point_f
(
    Matrix_f** x_fit,
    Vector_f** y_fit,
    Matrix_f*  x_nofit,
    Vector_f*  y_nofit,
    uint32_t   pt
)
{
    uint32_t num_rows, num_cols;
    uint32_t num_elts;
    uint32_t col;
    uint32_t elt;

    Matrix_f* new_x_fit = NULL;
    Vector_f* new_y_fit = NULL;

    num_rows = (*x_fit)->num_rows;
    num_cols = (*x_fit)->num_cols;

    create_matrix_f(&new_x_fit, num_rows+1, num_cols);
    copy_matrix_block_into_matrix_f(new_x_fit, 0, 0, *x_fit, 0, 0,
            num_rows, num_cols);
    free_matrix_f(*x_fit);
    *x_fit = new_x_fit;

    for (col = 0; col < num_cols; col++)
    {
        new_x_fit->elts[ num_rows ][ col ] = x_nofit->elts[ pt ][ col ];
    }

    num_elts = (*y_fit)->num_elts;

    create_vector_f(&new_y_fit, num_elts+1);
    for (elt = 0; elt < num_elts; elt++)
    {
        new_y_fit->elts[ elt ] = (*y_fit)->elts[ elt ];
    }
    free_vector_f(*y_fit);
    *y_fit = new_y_fit;

    new_y_fit->elts[ num_elts ] = y_nofit->elts[ pt ];
}

static void append_ransac_fit_point_d
(
    Matrix_d** x_fit,
    Vector_d** y_fit,
    Matrix_d*  x_nofit,
    Vector_d*  y_nofit,
    uint32_t   pt
)
{
    uint32_t num_rows, num_cols;
    uint32_t num_elts;
    uint32_t col;
    uint32_t elt;

    Matrix_d* new_x_fit = NULL;
    Vector_d* new_y_fit = NULL;

    num_rows = (*x_fit)->num_rows;
    num_cols = (*x_fit)->num_cols;

    create_matrix_d(&new_x_fit, num_rows+1, num_cols);
    copy_matrix_block_into_matrix_d(new_x_fit, 0, 0, *x_fit, 0, 0,
            num_rows, num_cols);
    free_matrix_d(*x_fit);
    *x_fit = new_x_fit;

    for (col = 0; col < num_cols; col++)
    {
        new_x_fit->elts[ num_rows ][ col ] = x_nofit->elts[ pt ][ col ];
    }

    num_elts = (*y_fit)->num_elts;

    create_vector_d(&new_y_fit, num_elts+1);
    for (elt = 0; elt < num_elts; elt++)
    {
        new_y_fit->elts[ elt ] = (*y_fit)->elts[ elt ];
    }
    free_vector_d(*y_fit);
    *y_fit = new_y_fit;

    new_y_fit->elts[ num_elts ] = y_nofit->elts[ pt ];
}

Error* linear_least_squares_ransac_regress_f
(
    Vector_f**      w_out,
    const Matrix_f* x,
    const Vector_f* y,
    uint32_t        n,
    uint32_t        k,
    float           t,
    uint32_t        d
)
{
    uint32_t i;
    uint32_t row;
    uint32_t elt;
    uint32_t N;
    uint32_t best_N;

    float r;
    float best_mse;
    float mse;

    Matrix_f* x_fit   = NULL;
    Matrix_f* x_nofit = NULL;
    Vector_f* y_fit   = NULL;
    Vector_f* y_nofit = NULL;
    Vector_f* r_fit   = NULL;
    Vector_f* r_nofit = NULL;
    Vector_f* best_w  = NULL;
    Error*    err;

    best_mse = -1;
    best_N = 0;

    for (i = 0; i < k; i++)
    {
        N = n;

        split_ransac_points_f(&x_fit, &y_fit, &x_nofit, &y_nofit, x, y, N);

        if ((err = linear_least_squares_regress_f(w_out, x_fit, y_fit)))
        {
            free_error(err);
            continue;
        }

        multiply_matrix_and_vector_f(&r_nofit, x_nofit, *w_out);

        for (row = 0; row < x_nofit->num_rows; row++)
        {
            r = fabs(y_nofit->elts[ row ] - r_nofit->elts[ row ]);

            if (r < t)
            {
                append_ransac_fit_point_f(&x_fit, &y_fit, x_nofit, y_nofit,row);

                N++;
            }
        }

        if (N >= d)
        {
            if ((err = linear_least_squares_regress_f(w_out, x_fit, y_fit)))
            {
                free_error(err);
                break;
            }

            mse = 0;
            multiply_matrix_and_vector_f(&r_fit, x_fit, *w_out);
            multiply_matrix_and_vector_f(&r_nofit, x_nofit, *w_out);
            for (elt = 0; elt < y_fit->num_elts; elt++)
            {
                mse += (y_fit->elts[ elt ] - r_fit->elts[ elt ]) *
                    (y_fit->elts[ elt ] - r_fit->elts[ elt ]);
            }
            mse /= N;

            if (mse < best_mse || best_mse < 0)
            {
                copy_vector_f(&best_w, *w_out);
                best_mse = mse;
                best_N = N;

                /*
                for (elt = 0; elt < r_nofit->num_elts; elt++)
                {
                    r = fabs(y_nofit->elts[ elt ] - r_nofit->elts[ elt ]);
                    fprintf(stdout, "%3d  %8.3f  %8.3f\n", elt, r, 
                            y_nofit->elts[ elt ]);
                }
                fprintf(stdout, "--\n");
                for (elt = 0; elt < r_fit->num_elts; elt++)
                {
                    r = fabs(y_fit->elts[ elt ] - r_fit->elts[ elt ]);
                    fprintf(stdout, "%3d  %8.3f  %8.3f\n", elt, r, 
                            y_fit->elts[ elt ]);
                }

                fprintf(stdout, "%3d  %2d  %6.1f\n\n", i, best_N, best_mse);
                */
            }
        }
    }

    free_vector_f(r_fit);
    free_vector_f(r_nofit);

    free_matrix_f(x_fit);
    free_vector_f(y_fit);
    free_matrix_f(x_nofit);
    free_vector_f(y_nofit);

    if (best_N < d || best_mse < 0)
    {
        free_vector_f(best_w);
        free_vector_f(*w_out); *w_out = NULL;
        return JWSC_ECALC("Could not fit with RANSAC criteria");
    }

    copy_vector_f(w_out, best_w);
    free_vector_f(best_w);

    return NULL;
}

Error* linear_least_squares_ransac_regress_d
(
    Vector_d**      w_out,
    const Matrix_d* x,
    const Vector_d* y,
    uint32_t        n,
    uint32_t        k,
    double          t,
    uint32_t        d
)
{
    uint32_t i;
    uint32_t row;
    uint32_t elt;
    uint32_t N;
    uint32_t best_N;

    double r;
    double best_mse;
    double mse;

    Matrix_d* x_fit   = NULL;
    Matrix_d* x_nofit = NULL;
    Vector_d* y_fit   = NULL;
    Vector_d* y_nofit = NULL;
    Vector_d* r_fit   = NULL;
    Vector_d* r_nofit = NULL;
    Vector_d* best_w  = NULL;
    Error*    err;

    best_mse = -1;
    best_N = 0;

    for (i = 0; i < k; i++)
    {
        N = n;

        split_ransac_points_d(&x_fit, &y_fit, &x_nofit, &y_nofit, x, y, N);

        if ((err = linear_least_squares_regress_d(w_out, x_fit, y_fit)))
        {
            free_error(err);
            continue;
        }

        multiply_matrix_and_vector_d(&r_nofit, x_nofit, *w_out);

        for (row = 0; row < x_nofit->num_rows; row++)
        {
            r = fabs(y_nofit->elts[ row ] - r_nofit->elts[ row ]);

            if (r < t)
            {
                append_ransac_fit_point_d(&x_fit, &y_fit, x_nofit, y_nofit,row);

                N++;
            }
        }

        if (N >= d)
        {
            if ((err = linear_least_squares_regress_d(w_out, x_fit, y_fit)))
            {
                free_error(err);
                break;
            }

            mse = 0;
            multiply_matrix_and_vector_d(&r_fit, x_fit, *w_out);
            multiply_matrix_and_vector_d(&r_nofit, x_nofit, *w_out);
            for (elt = 0; elt < y_fit->num_elts; elt++)
            {
                mse += (y_fit->elts[ elt ] - r_fit->elts[ elt ]) *
                       (y_fit->elts[ elt ] - r_fit->elts[ elt ]);
            }
            mse /= N;

            if (mse < best_mse || best_mse < 0)
            {
                copy_vector_d(&best_w, *w_out);
                best_mse = mse;
                best_N = N;

                /*
                for (elt = 0; elt < r_nofit->num_elts; elt++)
                {
                    r = fabs(y_nofit->elts[ elt ] - r_nofit->elts[ elt ]);
                    fprintf(stdout, "%3d  %8.3f  %8.3f\n", elt, r, 
                            y_nofit->elts[ elt ]);
                }
                fprintf(stdout, "--\n");
                for (elt = 0; elt < r_fit->num_elts; elt++)
                {
                    r = fabs(y_fit->elts[ elt ] - r_fit->elts[ elt ]);
                    fprintf(stdout, "%3d  %8.3f  %8.3f\n", elt, r, 
                            y_fit->elts[ elt ]);
                }

                fprintf(stdout, "%3d  %2d  %6.1f\n\n", i, best_N, best_mse);
                */
            }
        }
    }

    free_vector_d(r_fit);
    free_vector_d(r_nofit);

    free_matrix_d(x_fit);
    free_vector_d(y_fit);
    free_matrix_d(x_nofit);
    free_vector_d(y_nofit);

    if (best_N < d || best_mse < 0)
    {
        free_vector_d(best_w);
        free_vector_d(*w_out); *w_out = NULL;
        return JWSC_ECALC("Could not fit with RANSAC criteria");
    }

    copy_vector_d(w_out, best_w);
    free_vector_d(best_w);

    return NULL;
}

#endif