linear_system_helper.cpp File Reference

#include "linear_system_helper.h"
#include <random>
#include <algorithm>
#include <cassert>

Include dependency graph for linear_system_helper.cpp:

Go to the source code of this file.

Classes
struct	ThreadRansacArgs

Functions
void	solveLinearSystem (PseudoInverseHelper &h, Matrix1D< double > &result)
void	solveLinearSystem (WeightedLeastSquaresHelperMany &h, std::vector< Matrix1D< double >> &results)
void	weightedLeastSquares (WeightedLeastSquaresHelper &h, Matrix1D< double > &result)
void	weightedLeastSquares (WeightedLeastSquaresHelperMany &h, std::vector< Matrix1D< double >> &results)
double	ransacWeightedLeastSquaresBasic (WeightedLeastSquaresHelper &h, Matrix1D< double > &result, double tol, int Niter, double outlierFraction)
void *	threadRansacWeightedLeastSquares (void *args)
void	ransacWeightedLeastSquares (WeightedLeastSquaresHelper &h, Matrix1D< double > &result, double tol, int Niter, double outlierFraction, int Nthreads)

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;
}

ransacWeightedLeastSquaresBasic()

double ransacWeightedLeastSquaresBasic	(	WeightedLeastSquaresHelper &	h,
		Matrix1D< double > &	result,
		double	tol,
		int	Niter,
		double	outlierFraction
	)

Definition at line 164 of file linear_system_helper.cpp.

{
    int N=MAT_YSIZE(h.A); // Number of equations
    int M=MAT_XSIZE(h.A); // Number of unknowns

    // Initialize a vector with all equation indexes
    std::vector<int> eqIdx;
    eqIdx.reserve(N);
    for (int n=0; n<N; ++n)
        eqIdx.push_back(n);
    int *eqIdxPtr=&eqIdx[0];

#ifdef DEBUG_MORE
    // Show all equations
    for (int n=0; n<N; n++)
    {
        std::cout << "Eq. " << n << " w=" << VEC_ELEM(h.w,n) << " b=" << VEC_ELEM(h.b,n) << " a=";
        for (int j=0; j<M; j++)
            std::cout << MAT_ELEM(h.A,n,j) << " ";
        std::cout << std::endl;
    }
#endif

    // Resize a WLS helper for solving the MxM equation systems
    PseudoInverseHelper haux;
    haux.A.resizeNoCopy(M,M);
    haux.b.resizeNoCopy(M);
    Matrix2D<double> &A=haux.A;
    Matrix1D<double> &b=haux.b;

    // Solve Niter randomly chosen equation systems
    double bestError=1e38;
    const int Mdouble=M*sizeof(double);
    int minNewM=(int)((1.0-outlierFraction)*N-M);
    if (minNewM<0)
        minNewM=0;

    Matrix1D<double> resultAux;
    Matrix1D<int> idxIn(N);
    WeightedLeastSquaresHelper haux2;
    for (int it=0; it<Niter; ++it)
    {
#ifdef DEBUG_MORE
        std::cout << "Iter: " << it << std::endl;
#endif
        idxIn.initZeros();

        // Randomly select M equations
        std::random_device rd;
        std::mt19937 g(rd());
        std::shuffle(eqIdx.begin(), eqIdx.end(), g);

        // Select the equation system
        for (int i=0; i<M; ++i)
        {
            int idx=eqIdxPtr[i];
            memcpy(&MAT_ELEM(A,i,0),&MAT_ELEM(h.A,idx,0),Mdouble);
            VEC_ELEM(b,i)=VEC_ELEM(h.b,idx);
            VEC_ELEM(idxIn,idx)=1;
#ifdef DEBUG_MORE
        std::cout << "    Using Eq.: " << idx << " for first solution" << std::endl;
#endif
        }

        // Solve the equation system
        // We use LS because the weight of some of the equations might be too low
        // and then the system is ill conditioned
        solveLinearSystem(haux, resultAux);

        // Study the residuals of the rest
        int newM=0;
        for (int i=M+1; i<N; ++i)
        {
            int idx=eqIdxPtr[i];
            double bp=0;
            for (int j=0; j<M; ++j)
                bp+=MAT_ELEM(h.A,idx,j)*VEC_ELEM(resultAux,j);
            if (fabs(bp-VEC_ELEM(h.b,idx))<tol)
            {
                VEC_ELEM(idxIn,idx)=1;
                ++newM;
            }
#ifdef DEBUG_MORE
        std::cout << "    Checking Eq.: " << idx << " err=" << bp-VEC_ELEM(h.b,idx) << std::endl;
#endif
        }

        // If the model represent more points
        if (newM>minNewM)
        {
            Matrix2D<double> &A2=haux2.A;
            Matrix1D<double> &b2=haux2.b;
            Matrix1D<double> &w2=haux2.w;
            A2.resizeNoCopy(M+newM,M);
            b2.resizeNoCopy(M+newM);
            w2.resizeNoCopy(M+newM);

            // Select the equation system
            int targeti=0;
            for (int i=0; i<N; ++i)
                if (VEC_ELEM(idxIn,i))
                {
                    memcpy(&MAT_ELEM(A2,targeti,0),&MAT_ELEM(h.A,i,0),Mdouble);
                    VEC_ELEM(b2,targeti)=VEC_ELEM(h.b,i);
                    VEC_ELEM(w2,targeti)=VEC_ELEM(h.w,i);
                    ++targeti;
                }

            // Solve it with WLS
            weightedLeastSquares(haux2, resultAux);

            // Compute the mean error
            double err=0;
            for (int i=0; i<M+newM; ++i)
            {
                double bp=0;
                for (int j=0; j<M; ++j)
                    bp+=MAT_ELEM(A2,i,j)*VEC_ELEM(resultAux,j);
                err+=fabs(VEC_ELEM(b2,i)-bp)*VEC_ELEM(w2,i);
            }
            err/=(M+newM);
            if (err<bestError)
            {
                bestError=err;
                result=resultAux;
#ifdef DEBUG
                std::cout << "Best solution iter: " << it << " Error=" << err << " frac=" << (float)(M+newM)/VEC_XSIZE(h.b) << std::endl;
#ifdef DEBUG_MORE
                std::cout << "Result:" << result << std::endl;
                for (int i=0; i<M+newM; ++i)
                {
                    double bp=0;
                    for (int j=0; j<M; ++j)
                        bp+=MAT_ELEM(A2,i,j)*VEC_ELEM(resultAux,j);
                    std::cout << "Eq. " << i << " w=" << VEC_ELEM(w2,i) << " b2=" << VEC_ELEM(b2,i) << " bp=" << bp << std::endl;
                    err+=fabs(VEC_ELEM(b2,i)-bp)*VEC_ELEM(w2,i);
                }
#endif
#endif
            }
        }
    }
    return bestError;
}

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];
            }
        }
    }
}

threadRansacWeightedLeastSquares()

void* threadRansacWeightedLeastSquares

(

void *

args

)

Definition at line 324 of file linear_system_helper.cpp.

{
    ThreadRansacArgs * master = (ThreadRansacArgs *) args;
    master->error=ransacWeightedLeastSquaresBasic(*(master->h), master->result,
            master->tol, master->Niter, master->outlierFraction);
    return NULL;
}

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);
}