linear_system_helper.h File Reference

#include "matrix1d.h"
#include "matrix2d.h"

Include dependency graph for linear_system_helper.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
class	PseudoInverseHelper
class	WeightedLeastSquaresHelper
class	WeightedLeastSquaresHelperMany

Functions
void	solveLinearSystem (PseudoInverseHelper &h, Matrix1D< double > &result)
void	solveLinearSystem (WeightedLeastSquaresHelperMany &h, std::vector< Matrix1D< double >> &result)
void	weightedLeastSquares (WeightedLeastSquaresHelper &h, Matrix1D< double > &result)
void	weightedLeastSquares (WeightedLeastSquaresHelperMany &h, std::vector< Matrix1D< double >> &results)
void	ransacWeightedLeastSquares (WeightedLeastSquaresHelper &h, Matrix1D< double > &result, double tol, int Niter=10000, double outlierFraction=0.25, int Nthreads=1)

Function Documentation

ransacWeightedLeastSquares()

void ransacWeightedLeastSquares	(	WeightedLeastSquaresHelper &	h,
		Matrix1D< double > &	result,
		double	tol,
		int	Niter = 10000,
		double	outlierFraction = 0.25,
		int	Nthreads = 1
	)

Solve Weighted least square problem Ax=b and weights w, with RANSAC. Tol is a tolerance value: if an equation is fulfilled with an error smaller than tol, then it is considered to fulfill the model. Niter is the number of RANSAC iterations to perform. The outlier fraction is the fraction of equations that, at maximum, can be considered as outliers.

Definition at line 332 of file linear_system_helper.cpp.

{
    // Read and preprocess the images
    pthread_t * th_ids = new pthread_t[Nthreads];
    ThreadRansacArgs * th_args = new ThreadRansacArgs[Nthreads];
    for (int nt = 0; nt < Nthreads; nt++) {
        // Passing parameters to each thread
        th_args[nt].myThreadID = nt;
        th_args[nt].h = &h;
        th_args[nt].tol = tol;
        th_args[nt].Niter = Niter/Nthreads;
        th_args[nt].outlierFraction = outlierFraction;
        pthread_create((th_ids + nt), NULL, threadRansacWeightedLeastSquares,
                (void *) (th_args + nt));
    }

    // Waiting for threads to finish
    double err=1e38;
    for (int nt = 0; nt < Nthreads; nt++)
    {
        pthread_join(*(th_ids + nt), NULL);
        if (th_args[nt].error<err)
        {
            err=th_args[nt].error;
            result=th_args[nt].result;
        }
    }

    // Threads structures are not needed any more
    delete []th_ids;
    delete []th_args;
}

solveLinearSystem() [1/2]

void solveLinearSystem	(	PseudoInverseHelper &	h,
		Matrix1D< double > &	result
	)

Solve Linear system Ax=b with pseudoinverse. A and b must be set inside the PseudoInverseHelper, the rest of the fields in PseudoInverseHelper are used by this routine to avoid several allocation/deallocations DEPRECATED, use solveLinearSystem(WeightedLeastSquaresHelperMany &h, std::vector<Matrix1D<double>> &results)

Definition at line 33 of file linear_system_helper.cpp.

{
    Matrix2D<double> &A=h.A;
    Matrix1D<double> &b=h.b;
    Matrix2D<double> &AtA=h.AtA;
    Matrix2D<double> &AtAinv=h.AtAinv;
    Matrix1D<double> &Atb=h.Atb;

    // Compute AtA and Atb
    int I=MAT_YSIZE(A);
    int J=MAT_XSIZE(A);
    AtA.initZeros(J,J);
    Atb.initZeros(J);
    for (int i=0; i<J; ++i)
    {
        for (int j=0; j<J; ++j)
        {
            double AtA_ij=0;
            for (int k=0; k<I; ++k)
                AtA_ij+=MAT_ELEM(A,k,i)*MAT_ELEM(A,k,j);
            MAT_ELEM(AtA,i,j)=AtA_ij;
        }
        double Atb_i=0;
        for (int k=0; k<I; ++k)
            Atb_i+=MAT_ELEM(A,k,i)*VEC_ELEM(b,k);
        VEC_ELEM(Atb,i)=Atb_i;
    }

    // Compute the inverse of AtA
    AtA.inv(AtAinv);

    // Now multiply by Atb
    result.initZeros(J);
    FOR_ALL_ELEMENTS_IN_MATRIX2D(AtAinv)
        VEC_ELEM(result,i)+=MAT_ELEM(AtAinv,i,j)*VEC_ELEM(Atb,j);
}

solveLinearSystem() [2/2]

void solveLinearSystem	(	WeightedLeastSquaresHelperMany &	h,
		std::vector< Matrix1D< double >> &	result
	)

Solve Linear system Ax=[b] with pseudoinverse. A and all 'b's must be set inside the helper

Definition at line 70 of file linear_system_helper.cpp.

{
    const size_t sizeI = h.A.mdimy;
    const size_t sizeJ = h.A.mdimx;
    h.AtA.resizeNoCopy(sizeJ, sizeJ);
    h.Atb.resizeNoCopy(sizeJ);
    // compute AtA
    h.At = h.A.transpose();
    // for each row of the output
    for (size_t i = 0; i < sizeJ; ++i)
    {
        // for each column of the output (notice that we read row-wise from At)
        for (size_t j = i; j < sizeJ; ++j)
        {
            double AtA_ij = 0;
            // multiply elementwise row by row from At
            for (size_t k = 0; k < sizeI; ++k) {
                AtA_ij += h.At.mdata[i * sizeI + k] * h.At.mdata[j * sizeI + k];
            }
            // AtA is symmetric, so we don't need to iterate over everything
            h.AtA.mdata[i * sizeJ + j] = h.AtA.mdata[j * sizeJ + i] = AtA_ij;
        }
    }
    // Compute the inverse of AtA
    h.AtA.inv(h.AtAinv); // AtAinv is also square matrix, same size as AtA

    assert(results.size() == h.bs.size());
    auto res_it = results.begin();
    auto b_it = h.bs.begin(); 
    for (; res_it != results.end(); ++res_it, ++b_it) {
        for (size_t i = 0; i < sizeJ; ++i)
        {
            double Atb_i = 0;
            for (size_t k = 0; k < sizeI; ++k) {
                Atb_i += h.A.mdata[k * sizeJ + i] * b_it->vdata[k];
            }
            h.Atb.vdata[i] = Atb_i;
        }
        // Now multiply by Atb
        res_it->initZeros(sizeJ);
        for (size_t i = 0; i < sizeJ; i++) { 
            for (size_t j = 0; j < sizeJ; j++) {
                res_it->vdata[i] += h.AtAinv.mdata[i * sizeJ +j] * h.Atb.vdata[j];
            }
        }
    }
}

weightedLeastSquares() [1/2]

void weightedLeastSquares	(	WeightedLeastSquaresHelper &	h,
		Matrix1D< double > &	result
	)

Solve Weighted least square problem Ax=b with pseudoinverse and weights w. A, w and b must be set inside the WeightedLeastSquaresHelper, the rest of the fields in WeightedLeastSquaresHelper are used by this routine to avoid several allocation/deallocations.

The normal equations of this problem are A^t W A x = A^t W b, where W is a diagonal matrix whose entries are in the vector w.

DEPRECATED, use weightedLeastSquares(WeightedLeastSquaresHelperMany &h, std::vector<Matrix1D<double>> &results)

Definition at line 119 of file linear_system_helper.cpp.

{
    Matrix2D<double> &A=h.A;
    Matrix1D<double> &b=h.b;
    Matrix1D<double> &w=h.w;

    // See http://en.wikipedia.org/wiki/Least_squares#Weighted_least_squares
    FOR_ALL_ELEMENTS_IN_MATRIX1D(w)
    {
        double wii=sqrt(VEC_ELEM(w,i));
        VEC_ELEM(b,i)*=wii;
        for (size_t j=0; j<MAT_XSIZE(A); ++j)
            MAT_ELEM(A,i,j)*=wii;
    }
    solveLinearSystem(h,result);
}

weightedLeastSquares() [2/2]

void weightedLeastSquares	(	WeightedLeastSquaresHelperMany &	h,
		std::vector< Matrix1D< double >> &	results
	)

Solve Weighted least square problem Ax=b with pseudoinverse and weights w for multiple b. A, w and all 'b's must be set inside the helper to avoid several allocation/deallocations.

The normal equations of this problem are A^t W A x = A^t W b, where W is a diagonal matrix whose entries are in the vector w.

Definition at line 136 of file linear_system_helper.cpp.

{
    const size_t sizeW = h.w.vdim;
    h.w_sqrt.resizeNoCopy(h.w);
    // compute weights
    for(size_t i = 0; i < sizeW; ++i) {
        h.w_sqrt.vdata[i] = sqrt(h.w.vdata[i]);
    }
    // update the matrix
    const size_t sizeX = h.A.mdimx;
    for(size_t i = 0; i < sizeW; ++i) {
        const size_t offset = i * sizeX; 
        const auto v = h.w_sqrt[i];
        for (size_t j = 0; j < sizeX; ++j)
            h.A.mdata[offset + j] *= v;
    }
    // update values
    for (auto &b : h.bs) {
        for(size_t i = 0; i < sizeW; ++i) {
            b.vdata[i] *= h.w_sqrt[i];
        }
    }
    solveLinearSystem(h, results);
}