示例#1
0
void  cDistorBilin::Diff(ElMatrix<REAL> & aM,Pt2dr aP) const
{
    // InitEtatFromCorner(ToCoordGrid(aP));

    Pt2dr aPds;
    GetParamCorner(mCurCorner,aPds,ToCoordGrid(aP)); 
    const Pt2dr & aP00 =  Dist(mCurCorner+CamBilinCorn[0] ) ;
    const Pt2dr & aP10 =  Dist(mCurCorner+CamBilinCorn[1] ) ;
    const Pt2dr & aP01 =  Dist(mCurCorner+CamBilinCorn[2] ) ;
    const Pt2dr & aP11 =  Dist(mCurCorner+CamBilinCorn[3] ) ;


    Pt2dr aGx =    ((aP10-aP00)*aPds.y + (aP11-aP01)*(1-aPds.y))  / mStep.x;
    Pt2dr aGy =    ((aP01-aP00)*aPds.x + (aP11-aP10)*(1-aPds.x))  / mStep.y;

    aM.ResizeInside(2,2);
    SetCol(aM,0,aGx);
    SetCol(aM,1,aGy);

    // A conserver, verification par diff std
    if (0)
    {
        ElMatrix<REAL> aM2(2,2);
        DiffByDiffFinies(aM2,aP,euclid(mStep)/1e4);
        static double aDMax = 0;
        double aD = aM.L2(aM2);
        if (aD>aDMax)
        {
            aDMax = aD;
            std::cout << "DDDD " << aD << "\n";
        }
    }
    // InitEtatFromCorner(ToCoordGrid(aP));

}
// Compute the parameter of the transformation of Point in L coordinate
// to point in R coordinates
void CalcParamEqRel
(
    Pt3d<Fonc_Num> & aTr,
    ElMatrix<Fonc_Num> & aMat,
    cRotationFormelle & aRotR,
    cRotationFormelle & aRotL,
    int                 aRNumGl,
    int                 aLNumGl
)
{
#if (HandleGL != 1)
    ELISE_ASSERT(! aRotR.IsGL(),"Guimbal lock in Eq Rig still unsupported");
    ELISE_ASSERT(! aRotL.IsGL(),"Guimbal lock in Eq Rig still unsupported");
    aRNumGl=-1;
    aLNumGl=-1;
#endif

    ElMatrix<Fonc_Num> aRMat = aRotR.MatFGLComplete(aRNumGl);
    ElMatrix<Fonc_Num> aLMat = aRotL.MatFGLComplete(aLNumGl);
    ElMatrix<Fonc_Num> aRMatInv = aRMat.transpose();

    aMat = aRMatInv * aLMat;
    aTr = aRMatInv * (aRotL.COpt() - aRotR.COpt());

}
double cLiaisORPO::Residu()
{
    SetOrCam();
    if (! mProfCalc)
    {
       mProfCalc = true;
       SetProf();
    }

    // ElRotation3D aC1toM = mCam1.Orientation().inv();
    // ElRotation3D aMtoC2 = mCam2.Orientation();

    ElRotation3D  aC1toC2 =  mCam2.Orient() *  mCam1.Orient().inv();

    Pt3dr aBaseInC2 = aC1toC2.ImAff(Pt3dr(0,0,0));

    ElMatrix<double> aMat =  aC1toC2.Mat() ;

    Pt3dr aC0,aC1,aC2;
    aMat.GetCol(0,aC0);
    aMat.GetCol(1,aC1);
    aMat.GetCol(2,aC2);


    double sDist=0.0;
    int aK = 0;

    for (ElPackHomologue::const_iterator itP =mPack.begin() ; itP!=mPack.end(); itP++)
    {
       double aPds = itP->Pds();
       double aLambda =  mProfs.at(aK);

        Pt2dr aPIm1 = itP->P1();
        Pt3dr aRay1 = aC0 * aPIm1.x + aC1 * aPIm1.y + aC2;


        Pt3dr aPTerA = aBaseInC2 + aRay1 * (aLambda *0.99);
        Pt3dr aPTerB = aBaseInC2 + aRay1 * (aLambda *1.01);

        Pt2dr aProjA(aPTerA.x/aPTerA.z,aPTerA.y/aPTerA.z);
        Pt2dr aProjB(aPTerB.x/aPTerB.z,aPTerB.y/aPTerB.z);

         Pt2dr aVAB  (aProjB.y-aProjA.y,aProjA.x-aProjB.x);

         double aD2 =  ElAbs(scal(aVAB,aProjA-itP->P2()) / euclid(aVAB));
         sDist += aPds*aD2;

         aK++;
    }

    if (DEBUG_POWEL)
    {
        std::cout << sDist/mPack.size() << "\n"; // getchar();
    }

    return sDist * mPPL->Pds().Val();
}
cTypeCodageMatr ExportMatr(const ElMatrix<double> & aMat)
{
    cTypeCodageMatr  aCM;

    aMat.GetLig(0, aCM.L1() );
    aMat.GetLig(1, aCM.L2() );
    aMat.GetLig(2, aCM.L3() );
    aCM.TrueRot().SetVal(true);

    return aCM;
}
示例#5
0
void cElHomographie::ToMatrix(ElMatrix<REAL> &  M) const
{
   M.set_to_size(3,3);
   M(0,0) = mHX.CoeffX(); M(1,0) = mHX.CoeffY(); M(2,0) = mHX.Coeff1();
   M(0,1) = mHY.CoeffX(); M(1,1) = mHY.CoeffY(); M(2,1) = mHY.Coeff1();
   M(0,2) = mHZ.CoeffX(); M(1,2) = mHZ.CoeffY(); M(2,2) = mHZ.Coeff1();
}
void cGenSysSurResol::Indexee_SoustraitMatrColInLin
     (
           const ElMatrix<tSysCho> &aMatr,
	   const std::vector<cSsBloc> & aVBl
     )
{
    ELISE_ASSERT ( aMatr.tx()==1, "matrice non colonne, Indexee_EcrireMatrColWithLin");

    int anXIn=0;
    for (int aKBl=0 ; aKBl <int(aVBl.size()) ; aKBl++)
    {
        int aI0x = aVBl[aKBl].I0AbsSolve();
        int aI1x = aVBl[aKBl].I1AbsSolve();
        for (int anXOut = aI0x; anXOut < aI1x ; anXOut++)
        {
	     SetElemLin(anXOut,GetElemLin(anXOut)-aMatr(0,anXIn));
             // GetElemLin(anXOut) -= aMatr(0,anXIn);
             anXIn++;
        }
    }
/*
    for (int aK=0 ; aK<int(aVInd.size()) ; aK++)
    {
	GetElemLin(aVInd[aK]) -= aMatr(0,aK);
    }
*/
}
static Pt3dr  PInBCoord(const Pt3dr & aBase,const ElMatrix<double> & aRot,const Pt3dr & aPA)
{
  // mDirOr1A (vunit(mBase^mDir1A)),
    Pt3dr aRes =  aRot.transpose() * (aBase+aPA);

     std::cout << "PInBCoord " << scal(aBase^aPA,aRot*aRes) << "\n";
    return aRes;
}
ElMatrix<double> cOriFromBundle::CalculRot(const cOFB_Sol1S1T & aSol,bool Show)
{
   // R [mDir1B ...] = [mV1 ...]

    ElMatrix<double>  aMatB =     MatFromCol(mDir1B,mDir2B,mDir3B);
    ElMatrix<double> aMatBinA =   MatFromCol(aSol.mV1,aSol.mV2,aSol.mV3);
    ElMatrix<double> aMat = aMatBinA * gaussj(aMatB);

// std::cout << mDir1B << mDir2B << mDir3B << "\n";
// std::cout << aSol.mV1 << aSol.mV2 << aSol.mV3 << "\n";
// std::cout << "xxxxxxxxxxxx\n";
/*
    for (int anY=0 ; anY<3 ; anY++)
    {
        for (int anX=0 ; anX<3 ; anX++)
        {
            std::cout << aMat(anX,anY) << " ";
        }
        std::cout << "\n";
    }
*/
// getchar();




    ElMatrix<double> aR = NearestRotation(aMat);
// std::cout << "yyyyyyyyyyyyyy\n";


    if (Show)
    {
        std::cout << "DET " << aR.Det() -1.0  << " Eucl " << ElMatrix<double>(3,true).L2(aR*aR.transpose())<< "\n";
        std::cout  << " SCAL " << scal(mBase^mDir1A,aR*mDir1B)  << " "
                               << scal(mBase^mDir2A,aR*mDir2B)  << " "
                               << scal(mBase^mDir3A,aR*mDir3B)  << "\n";
            
    }
    
    return aR;
}
void cGenSysSurResol::Indexee_UpdateLinWithMatrCol
     (
           const ElMatrix<tSysCho> &aMatr,
	   const std::vector<INT> & aVInd
     )
{
    ELISE_ASSERT ( aMatr.tx()==1, "matrice non colonne, Indexee_EcrireMatrColWithLin");

    for (int aK=0 ; aK<int(aVInd.size()) ; aK++)
    {
	SetElemLin(aVInd[aK],aMatr(0,aK));
    }
}
示例#10
0
void L2SysSurResol::Indexee_EcrireDansMatrWithQuad
     (
            ElMatrix<double> &aMatr,
            const std::vector<cSsBloc> &  aVx,
            const std::vector<cSsBloc> &  aVy
     )  const
{
   // cGenSysSurResol::Indexee_EcrireDansMatrWithQuad(aMatr,aVx,aVy);

    double ** aDOut = aMatr.data();
    int xout=0;
    for (int aKx=0 ; aKx <int(aVx.size()) ; aKx++)
    {
        const cSsBloc & aBlX = aVx[aKx];
        int aI0x = aBlX.I0AbsSolve();
        int aNbX = aBlX.Nb();
        int yout=0;
        for (int aKy=0 ; aKy <int(aVy.size()) ; aKy++)
        {
            const cSsBloc & aBlY = aVy[aKy];
            int aI0y = aVy[aKy].I0AbsSolve();
            if ((aI0x>=aI0y) || (!mOptSym))
            {
                int aI1y = aBlY.I1AbsSolve();
                for (int aYIn=aI0y ; aYIn<aI1y ; aYIn++)
                {
                    memcpy
                    (
                        aDOut[yout]+xout,
                        mDatatLi_Li[aYIn]+aI0x,
                        aNbX*sizeof(double)
                    );
                    yout++;
                }
            }
            else
            {
                yout += aBlY.Nb();
            }
        }
        xout += aNbX;
    }

}
示例#11
0
void L2SysSurResol::Indexee_EcrireDansMatrColWithLin
              (ElMatrix<double> &aMatr,const std::vector<cSsBloc> &  aVy) const
{

   // cGenSysSurResol::Indexee_EcrireDansMatrColWithLin(aMatr,aVy);

    double ** aDOut = aMatr.data();
    int yout = 0;
    for (int aKy=0 ; aKy <int(aVy.size()) ; aKy++)
    {
        int aI0y = aVy[aKy].I0AbsSolve();
        int aI1y = aVy[aKy].I1AbsSolve();
        for (int yin = aI0y; yin < aI1y ; yin++)
        {
            aDOut[yout][0] = mDatabi_Li[yin];
            yout ++;
        }
    }

}
示例#12
0
void cGenSysSurResol::Indexee_EcrireDansMatrColWithLin
     (
           ElMatrix<tSysCho> &aMatr,
	   const std::vector<cSsBloc> & aVx
     ) const
{
    ELISE_ASSERT ( aMatr.tx()==1, "matrice non colonne, Indexee_EcrireMatrColWithLin");

    int xout = 0;
    for (int aKx=0 ; aKx <int(aVx.size()) ; aKx++)
    {
        int aI0x = aVx[aKx].I0AbsSolve();
        int aI1x = aVx[aKx].I1AbsSolve();
        for (int xin = aI0x; xin < aI1x ; xin++)
        {
            aMatr(0,xout) = GetElemLin(xin);
            xout ++;
        }
    }
}
示例#13
0
void correctPlanarPolygon( vector<Pt3dr> &aPolygon )
{
	if (aPolygon.size() < 3) ELISE_ERROR_EXIT("aPolygon.size() = " << aPolygon.size() << " < 3");
	Pt3dr u = vunit(aPolygon[1] - aPolygon[0]), minV = vunit(aPolygon[2] - aPolygon[0]);
	size_t minI = 2;
	REAL minScal = ElAbs(scal(u, minV));
	for (size_t i = 3; i < aPolygon.size(); i++)
	{
		Pt3dr v = vunit(aPolygon[i] - aPolygon[0]);
		REAL d = ElAbs(scal(u, v));
		if (d < minScal)
		{
			minScal = d;
			minI = i;
			minV = v;
		}
	}
	cout << "minI = " << minI << endl;
	cout << "minScal = " << minScal << endl;
	cout << "minV = " << minV << endl;
	Pt3dr n = u ^ minV;
	cout << "minV = " << minV << endl;
	ElMatrix<REAL> planToGlobalMatrix = MatFromCol(u, minV, n);
	if (planToGlobalMatrix.Det() < 1e-10) ELISE_ERROR_EXIT("matrix is not inversible");

	ElRotation3D planToGlobalRot(aPolygon[0], planToGlobalMatrix, true);
	ElRotation3D globalToPlanRot = planToGlobalRot.inv();

	//~ const size_t nbVertices = aPolygon.size();
	//~ ostringstream ss;
	//~ static int ii = 0;
	//~ const REAL extrudSize = 1e4;
	//~ ss << "polygon_" << (ii++) << ".ply";
	//~ ofstream f(ss.str().c_str());
	//~ f << "ply" << endl;
	//~ f << "format ascii 1.0" << endl;
	//~ f << "element vertex " << 4 * nbVertices << endl;
	//~ f << "property float x" << endl;
	//~ f << "property float y" << endl;
	//~ f << "property float z" << endl;
	//~ f << "property uchar red" << endl;
	//~ f << "property uchar green" << endl;
	//~ f << "property uchar blue" << endl;
	//~ f << "element face " << nbVertices << endl;
	//~ f << "property list uchar int vertex_indices" << endl;
	//~ f << "end_header" << endl;

	REAL zDiffSum = 0.;
	for (size_t i = 0; i < aPolygon.size(); i++)
	{
		Pt3dr p = globalToPlanRot.ImAff(aPolygon[i]);
		zDiffSum += ElAbs(p.z);
		aPolygon[i] = planToGlobalRot.ImAff(Pt3dr(p.x, p.y, 0.));

		//~ Pt3dr p0 = (i == 0) ? aPolygon[aPolygon.size() - 1] : aPolygon[i - 1], p1 = aPolygon[i], p2 = p1 + n * extrudSize, p3 = p0 + n * extrudSize;
		//~ f << p0.x << ' ' << p0.y << ' ' << p0.z << " 128 128 128" << endl;
		//~ f << p1.x << ' ' << p1.y << ' ' << p1.z << " 128 128 128" << endl;
		//~ f << p2.x << ' ' << p2.y << ' ' << p2.z << " 128 128 128" << endl;
		//~ f << p3.x << ' ' << p3.y << ' ' << p3.z << " 128 128 128" << endl;
	}

	//~ for (size_t i = 0; i < aPolygon.size(); i++)
		//~ f << 4 << ' ' << i * 4 << ' ' << i * 4 + 1 << ' ' << i * 4 + 2 << ' ' << i* 4 + 3 << endl;
	//~ f.close();
	cout << "zDiffSum = " << zDiffSum << endl;
}
示例#14
0
void  L2SysSurResol::SoutraitProduc3x3
     ( 
                          bool                   Sym,
                          ElMatrix<double> &aM1, 
                          ElMatrix<double> &aM2, 
                          const std::vector<cSsBloc> * aYVSB

     )
{
   double ** aData1 = aM1.data();
   double ** aData2 = aM2.data();

   double * aL2A = aData2[0];
   double * aL2B = aData2[1];
   double * aL2C = aData2[2];
   int aNbBl = aYVSB->size();
   int Y0InBloc = 0;
   for (int aKBly=0 ; aKBly <aNbBl ; aKBly++)
   {
        const cSsBloc & aBlY = (*aYVSB)[aKBly];
        const cIncIntervale * aIntY  = aBlY.Intervale();
        int aNumBlocIntervY    = aIntY->NumBlocSolve();

        int aI0y = aBlY.I0AbsSolve();
        int aI1y = aBlY.I1AbsSolve();

        int X0InBloc =0;
        for (int aKBlx=0 ; aKBlx <aNbBl ; aKBlx++)
        {
            const cSsBloc & aBlX = (*aYVSB)[aKBlx];
            const cIncIntervale * aIntX  = aBlX.Intervale();
            int aNumBlocIntervX    = aIntX->NumBlocSolve();
            if ((!mOptSym) || (aNumBlocIntervX>=aNumBlocIntervY))
            {
               int aI0x = aBlX.I0AbsSolve();
               int aI1x = aBlX.I1AbsSolve();
               int yin = Y0InBloc;

               for (int aYOut = aI0y; aYOut<aI1y ; aYOut++,yin++)
               {
                   double * aLineOut = mDatatLi_Li[aYOut];
                   double A1 = aData1[yin][0];
                   double B1 = aData1[yin][1];
                   double C1 = aData1[yin][2];

                   int aDebX  = (mOptSym && (aNumBlocIntervX==aNumBlocIntervY)) ? aYOut : aI0x;

                   int xin = X0InBloc + aDebX-aI0x;
                   for (int XOut = aDebX ; XOut<aI1x ; XOut++)
                   {
                        aLineOut[XOut] -= A1 * aL2A[xin] + B1 * aL2B[xin] + C1 *aL2C[xin];
                       xin++;
                   }
               }

            }
            X0InBloc += aBlX.Nb();
        }

        Y0InBloc += aBlY.Nb();
   }
}
示例#15
0
void cGenSysSurResol::SoutraitProduc3x3
     ( 
                          bool                   Sym,
                          ElMatrix<tSysCho> &aM1,
                          ElMatrix<tSysCho> &aM2,
                          const std::vector<cSsBloc> * aYVSB

     )
{
// std::cout << "AV 333x3333\n";

    tSysCho ** aData1 = aM1.data();
    tSysCho ** aData2 = aM2.data();

    tSysCho * aL2A = aData2[0];
    tSysCho * aL2B = aData2[1];
    tSysCho * aL2C = aData2[2];


/*
   // std::cout << "AAAAAAAAAAAAAAAAaakkkkkkkkkkkkkk\n";
   if ((!aYVSB) ||   (aYVSB->size()!=1))
   {
       std::cout << "aYVSB: " << aYVSB << "\n";
       std::cout << " size: " << aYVSB->size() << "\n";
       ELISE_ASSERT(aYVSB && (aYVSB->size()==1),"cGenSysSurResol::SoutraitProduc3x3");
   }
*/
       ELISE_ASSERT(aYVSB,"cGenSysSurResol::SoutraitProduc3x3");

       int aY=0;
       for (int aKBy=0 ; aKBy <int(aYVSB->size()) ; aKBy++)
       {
           const cSsBloc &  aBlY = (*aYVSB)[aKBy];
           for (int aKy=aBlY.I0AbsSolve() ; aKy <aBlY.I1AbsSolve() ; aKy++)
           {
              int aDebX = Sym ? aY : 0;
              tSysCho A1 = aData1[aY][0];
              tSysCho B1 = aData1[aY][1];
              tSysCho C1 = aData1[aY][2];

              int aX=0;

              for (int aKBx=0 ; aKBx <int(aYVSB->size()) ; aKBx++)
              {
                  const cSsBloc &  aBlX = (*aYVSB)[aKBx];
                  for (int aKx=aBlX.I0AbsSolve() ; aKx <aBlX.I1AbsSolve() ; aKx++)
                  {
                      if (aX>=aDebX)
                      {
	                 SetElemQuad(aKx,aKy,GetElemQuad(aKx,aKy) -(A1 * aL2A[aX] + B1 * aL2B[aX] + C1 *aL2C[aX]));
                      }
                      aX++;
                  }
              }
/*
              for (int aKx=aDebX ; aKx <int(aVIndy.size()) ; aKx++)
              {
	          GetElemQuad(aVIndy[aKx],aVIndy[aKy]) -= A1 * aL2A[aKx] + B1 * aL2B[aKx] + C1 *aL2C[aKx];
              }
*/
              aY++;
           }
       }

/*
    for (int aKy=0 ; aKy <int(aVIndy.size()) ; aKy++)
    {
        int aDebX = Sym ? aKy : 0;
        double A1 = aData1[aKy][0];
        double B1 = aData1[aKy][1];
        double C1 = aData1[aKy][2];

        for (int aKx=aDebX ; aKx <int(aVIndy.size()) ; aKx++)
        {
	    GetElemQuad(aVIndy[aKx],aVIndy[aKy]) -= A1 * aL2A[aKx] + B1 * aL2B[aKx] + C1 *aL2C[aKx];
        }
    }
*/


// std::cout << "AV 333x3333\n";
}