plane estimation

kplanes.hpp

vec4 plane3points(vec3 u,vec3 v,vec3 w)
{
//the function returns a 4-vector with the plane equation coefficients
//assuming the form  ax + by + cz + d = 0
vec4 res;
vec3 p1,p2,n;
vec d;
        p1 = normalise(v - u);
        p2 = normalise(w - u);
        n  = normalise(cross(p1,p2));
        d = - dot(n,u);

res = join_vert(n,d);

//cout << res <<endl;
return res;
}


vec4 leasqPlane(vector<vec3> pointList,string repNam,uword ijpt=0)
{
vec4 res,Xo;
mat data,P,Pinf,A,B,M,mvcXa,mvcLa,mvcV,mvcW;
vec Lb,W,X,Xa,K,V,La;
double vp;

if (pointList.size() >= 3)
{Xo = plane3points(pointList.at(0),pointList.at(1),pointList.at(2));}


Pinf = eye(3,3)*10000;

    if (pointList.size() < 3)
    {
    res = {0,0,1,0};
    }
    else if (pointList.size() == 3)
    {
    res = Xo;
    }
    else
    {
        ofstream out(repNam);


    //vetor Lb
    data = ones(pointList.size(),3);

    for (uword i0 = 0; i0 < pointList.size();i0++)
    {
    data.row(i0) = pointList.at(i0).t();
    }

    Lb = vectorise(data.t());

    //matriz dos pesos
    P = eye(Lb.n_elem,Lb.n_elem);
        if (ijpt != 0)
        {
        //em caso de que haja um ponto a ser injuncionado
//        P.submat(ijpt*3-2,ijpt*3-2,ijpt*3,ijpt*3) = Pinf;
        P.submat(ijpt*3-3,ijpt*3-3,ijpt*3-1,ijpt*3-1) = Pinf;
        }
    //matriz jacobiana A (parametros)

    A = ones<mat>(pointList.size(),4);

    for (uword i = 0; i < pointList.size();i++)
    {
    A(i,0)=data(i,0);
    A(i,1)=data(i,1);
    A(i,2)=data(i,2);
    }

    //matriz jacobiana B (observacoes)

    B = zeros<mat>(pointList.size(),Lb.n_elem);

    uword j = 0;
    for (uword i2 = 0;i2 < pointList.size();i2++)
    {
    B(i2,0+j)=Xo(0);
    B(i2,1+j)=Xo(1);
    B(i2,2+j)=Xo(2);

    j += 3;
    }

    W = zeros<vec>(pointList.size());

        for (uword i3 = 0;i3 < pointList.size();i3++)
        {
        W(i3) = dot(A.row(i3),Xo);
        }

    M = B * P.i() * B.t();

    X = - inv(A.t()*M.i()*A)*(A.t()*M.i()*W);

    Xa = Xo + X;

    K = -M.i()*(A*X+W);

    V = P.i() * B.t() * K;

    La = Lb+V;

    vp = as_scalar( V.t() * P * V / (Lb.n_elem - Xo.n_elem) );

    mvcXa = vp * inv(A.t()*M.i()*A);

    mvcLa = vp*(inv(P)+inv(P)*B.t()*inv(M)*A*inv(A.t()*inv(M)*A)*A.t()*inv(M)*B*inv(P)-inv(P)*B.t()*inv(M)*B*inv(P));

    mvcV  = vp*P.i()-mvcLa;

    mvcW  = vp*M;

out.precision(30);


    res = Xa;
    
    //normalizing the plane equations
    res.rows(0,2) = normalise(Xa.rows(0,2));
    res(3) = - dot(normalise(Xa.rows(0,2)),La.rows(0,2));

    data.print(out,"dados de entrada: ");out<<endl;

    res.print(out,"coeficientes do plano estimado: ");out<<endl;

    V.print(out,"resíduos: ");out<<endl;

    out << "var. posteriori: " << vp << endl<<endl;

    mvcXa.print(out,"MVC dos parametros ajustados");out<<endl;

    mvcLa.print(out,"MVC das observações ajustadas");out<<endl;

    }
cout<< res<<endl;
return res;
}