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;
}
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));
}
}
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;
}
}
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 ++;
}
}
}
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 ++;
}
}
}
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;
}
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();
}
}
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";
}