int OCCEdge::createArc(OCCVertex *start, OCCVertex *end, OCCStruct3d center) {
try {
gp_Pnt aP1(start->X(), start->Y(), start->Z());
gp_Pnt aP2(center.x, center.y, center.z);
gp_Pnt aP3(end->X(), end->Y(), end->Z());
Standard_Real Radius = aP1.Distance(aP2);
gce_MakeCirc MC(aP2,gce_MakePln(aP1, aP2, aP3).Value(), Radius);
const gp_Circ& Circ = MC.Value();
Standard_Real Alpha1 = ElCLib::Parameter(Circ, aP1);
Standard_Real Alpha2 = ElCLib::Parameter(Circ, aP3);
Handle(Geom_Circle) C = new Geom_Circle(Circ);
Handle(Geom_TrimmedCurve) arc = new Geom_TrimmedCurve(C, Alpha1, Alpha2, false);
this->setShape(BRepBuilderAPI_MakeEdge(arc, start->vertex, end->vertex));
} catch(Standard_Failure &err) {
Handle_Standard_Failure e = Standard_Failure::Caught();
const Standard_CString msg = e->GetMessageString();
if (msg != NULL && strlen(msg) > 1) {
setErrorMessage(msg);
} else {
setErrorMessage("Failed to create arc");
}
return 0;
}
return 1;
}
template <class cCRNode> void cTplCoxRoyAlgo<cCRNode>::SetStdCostRegul(double aCoeff,double aCste,int aVmin)
{
for (int anX=X0(); anX<X1() ; anX++)
for (int anY=Y0(); anY<Y1() ; anY++)
for (int aZ = ZMin(anX,anY); aZ< ZMax(anX,anY) ; aZ++)
{
cRoyPt aP1 (anX,anY,aZ);
cCRNode & aS1 = NodeOfP(aP1);
int aC1 = aS1.ResidualFlow(mDirZPlus);
for (int anEdg=0; anEdg<NbEdges() ; anEdg++)
{
if (aS1.EdgeIsValide(anEdg) && (!tabCRIsVertical[anEdg]))
{
cRoyPt aP2(aP1,anEdg);
cCRNode & aS2 = NodeOfP(aP2);
int aC2 = aS2.ResidualFlow(mDirZPlus);
double aCost = aCste + aCoeff*(aC1+aC2)/2.0;
if (Cnx8())
aCost *= tabCRIsArcV8[anEdg] ? 0.2928 : 0.4142 ;
int iCost = int(aCost+0.5);
if (iCost<aVmin)
iCost = aVmin;
aS1.SetResidualFlow(anEdg,iCost);
}
}
}
}
void BiScroller::MasqMakeOneLine
(
U_INT1 * aDataImR,
U_INT1 * aDataImG,
U_INT1 * aDataImB,
INT aY,
INT anX0,
INT anX1,
U_INT1 * aLine1,
U_INT1 * aLine2
)
{
static U_INT1 * aLR=0;
static U_INT1 * aLG=0;
static U_INT1 * aLB=0;
if (aLR==0)
{
aLR = new U_INT1 [256] ;
aLG = new U_INT1 [256] ;
aLB = new U_INT1 [256] ;
// Pt3dr aP0(128,0,0);
// Pt3dr aP1(255,128,255);
// Pt3dr aP0(0,0,128);
// Pt3dr aP1(255,128,255);
Pt3dr aP0(0,80,0);
Pt3dr aP1(255,255,128);
for (int aK=0 ; aK< 256 ; aK++)
{
double aPds0 = 1- aK /255.0;
double aPds1 = 1-aPds0;
Pt3dr aCol = aP0 *aPds0 + aP1*aPds1;
aLR[aK] = round_ni(aCol.x);
aLG[aK] = round_ni(aCol.y);
aLB[aK] = round_ni(aCol.z);
}
}
TIm2D<U_INT1,INT> aTM(mImMasq);
Pt2di aP(anX0,aY);
Pt2dr aQ0 = mScrGray->to_user(Pt2dr(aP));
Pt2dr aQ1 = mScrGray->to_user(Pt2dr(aP)+Pt2dr(1,0));
double aDx = aQ1.x -aQ0.x;
double aXQ0 = aQ0.x;
int aYQ0 = round_ni(aQ0.y);
for (; aP.x<anX1 ; aP.x++)
{
bool inM = (aTM.get(Pt2di(round_ni(aXQ0),aYQ0),0)!=0);
int aCoul = aLine1[aP.x];
aDataImR[aP.x] = inM?aLR[aCoul]:aCoul ;
aDataImG[aP.x] = inM?aLG[aCoul]:aCoul ;
aDataImB[aP.x] = inM?aLB[aCoul]:aCoul ;
aXQ0 +=aDx;
}
}
NS_RHH_BEGIN
/*************************************************/
/* */
/* cImagH */
/* */
/*************************************************/
double cAppliReduc::ErrorSolLoc()
{
double aSomEr = 0.0;
double aSomP = 0.0;
for (int aKIm1=0 ; aKIm1<int(mIms.size()) ; aKIm1++)
{
cImagH * anI1 = mIms[aKIm1];
cElHomographie aCurH1 = anI1->HF()->HomCur();
if (anI1->C2CI())
{
double aPdsE = 1 / anI1->PdsEchant();
const std::vector<cLink2Img*> & aVL = anI1->VLink();
for (int aKL=0 ; aKL<int(aVL.size()) ; aKL++)
{
cLink2Img * aLnk = aVL[aKL];
cImagH* anI2 = aLnk->Dest();
cElHomographie aCurH2 = anI2->HF()->HomCur();
cElHomographie aCurH2Inv = aCurH2.Inverse();
if (anI2->C2CI())
{
cElHomographie aH12 = aLnk->Hom12();
const std::vector<Pt3dr> & anEch = aLnk->EchantP1();
for (int aKEch = 0 ; aKEch< int(anEch.size()) ; aKEch++)
{
Pt3dr aP3d = anEch[aKEch];
Pt2dr aP1(aP3d.x,aP3d.y);
Pt2dr aP2 = aH12.Direct(aP1);
double aPds = aP3d.z * aPdsE;
Pt2dr aRes = aCurH2Inv.Direct(aCurH1.Direct(aP1)) - aP2;
double anEr = square_euclid(aRes);
aSomEr+= anEr * aPds;
aSomP+= aPds;
}
}
}
}
}
return sqrt(aSomEr/aSomP);
}
void PbHom(const std::string & anOri)
{
const std::string & aDir = "/media/data1/Calib-Sony/FacadePlane-2000/";
const std::string & aIm1 = "DSC05180.ARW";
const std::string & aIm2 = "DSC05182.ARW";
cInterfChantierNameManipulateur * aICNM = cInterfChantierNameManipulateur::BasicAlloc(aDir);
std::string aKeyOri = "NKS-Assoc-Im2Orient@-" + anOri;
std::string aNameOri1 = aICNM->Assoc1To1(aKeyOri,aIm1,true);
std::string aNameOri2 = aICNM->Assoc1To1(aKeyOri,aIm2,true);
CamStenope * aCS1 = CamOrientGenFromFile(aNameOri1,aICNM);
CamStenope * aCS2 = CamOrientGenFromFile(aNameOri2,aICNM);
Pt2dr aP1 (774,443);
Pt2dr aP2 (5541,3758);
Pt3dr aTer1 = aCS1->ImEtProf2Terrain(aP1,1.0);
Pt2dr aProj1 = aCS1->R3toF2(aTer1);
std::cout << "P & Proj Init1" << aP1 << aProj1 << " " << euclid(aP1-aProj1) << "\n";
Pt3dr aTer2 = aCS2->ImEtProf2Terrain(aP2,1.0);
Pt2dr aProj2 = aCS2->R3toF2(aTer2);
std::cout << "P & Proj Init2" << aP2 << aProj2 << " " << euclid(aP2-aProj2) << "\n";
double aDist;
Pt3dr aTerInter = aCS1->PseudoInter(aP1,*aCS2,aP2,&aDist);
aProj1 = aCS1->R3toF2(aTerInter);
aProj2 = aCS2->R3toF2(aTerInter);
std::cout << "Proj Inter " << aDist << " " << (aP1-aProj1) << " " << (aP2-aProj2) << "\n";
std::cout << "\n";
}
int OCCEdge::createLine(OCCVertex *start, OCCVertex *end) {
try {
gp_Pnt aP1(start->X(), start->Y(), start->Z());
gp_Pnt aP2(end->X(), end->Y(), end->Z());
GC_MakeLine line(aP1, aP2);
this->setShape(BRepBuilderAPI_MakeEdge(line, start->vertex, end->vertex));
} catch(Standard_Failure &err) {
Handle_Standard_Failure e = Standard_Failure::Caught();
const Standard_CString msg = e->GetMessageString();
if (msg != NULL && strlen(msg) > 1) {
setErrorMessage(msg);
} else {
setErrorMessage("Failed to create line");
}
return 0;
}
return 1;
}
int OCCEdge::createArc3P(OCCVertex *start, OCCVertex *end, OCCStruct3d aPoint) {
try {
gp_Pnt aP1(start->X(), start->Y(), start->Z());
gp_Pnt aP2(aPoint.x, aPoint.y, aPoint.z);
gp_Pnt aP3(end->X(), end->Y(), end->Z());
GC_MakeArcOfCircle arc(aP1, aP2, aP3);
this->setShape(BRepBuilderAPI_MakeEdge(arc, start->vertex, end->vertex));
} catch(Standard_Failure &err) {
Handle_Standard_Failure e = Standard_Failure::Caught();
const Standard_CString msg = e->GetMessageString();
if (msg != NULL && strlen(msg) > 1) {
setErrorMessage(msg);
} else {
setErrorMessage("Failed to create arc");
}
return 0;
}
return 1;
}
int OCCEdge::createCircle(OCCStruct3d center, OCCStruct3d normal, double radius)
{
try {
gp_Pnt aP1(center.x, center.y, center.z);
gp_Dir aD1(normal.x, normal.y, normal.z);
if (radius <= Precision::Confusion()) {
StdFail_NotDone::Raise("radius to small");
}
gce_MakeCirc circle(aP1, aD1, radius);
this->setShape(BRepBuilderAPI_MakeEdge(circle));
} catch(Standard_Failure &err) {
Handle_Standard_Failure e = Standard_Failure::Caught();
const Standard_CString msg = e->GetMessageString();
if (msg != NULL && strlen(msg) > 1) {
setErrorMessage(msg);
} else {
setErrorMessage("Failed to create circle");
}
return 0;
}
return 1;
}
void cAppliReduc::AmelioHomLocal(cImagH & anIm)
{
double aPdsLVMStd = 0.1;
double aPdsFreezC = 100;
double aPdsEvol = 10;;
int aNbIterSupl = 3;
int aMaxIterProgr = 6;
const std::vector<cLink2Img*> & aVLImC = mImCAmel->VLink();
for (int aKIm=0 ; aKIm<int(mIms.size()) ; aKIm++)
{
mIms[aKIm]->HF()->ReinitHom(cElHomographie::Id());
mIms[aKIm]->GainLoc() = 0;
mIms[aKIm]->InitLoc() = false;
}
std::vector<cImagH *> aVIms ;
for (int aKL=0 ; aKL<int(aVLImC.size()) ; aKL++)
{
cLink2Img * aLnK = aVLImC[aKL];
cImagH * anIm = aLnK->Dest();
anIm->GainLoc() = aLnK->PdsEchant() + 1e-7;
anIm->C2CI() = true;
aVIms.push_back(anIm);
anIm->HF()->ReinitHom(aLnK->Hom12().Inverse());
}
mImCAmel->GainLoc() = 1e10;
mImCAmel->InitLoc() = true;
mImCAmel->C2CI() = true;
int aNbIm2Init = aVIms.size();
cCmpPtrImOnGain aCmpPtrIm;
std::sort(aVIms.begin(),aVIms.end(),aCmpPtrIm);
int aNbIterProgr = std::min(aMaxIterProgr,round_up(aVIms.size()/3.0));
int aNbIterTot = aNbIterProgr + aNbIterSupl;
double aErrorIn = ErrorSolLoc();
if (Show(eShowGlob))
std::cout << "ERROR IN " << aErrorIn << "\n";
for (int aNbIter =0 ; aNbIter < aNbIterTot ; aNbIter ++)
{
if (aNbIter < aNbIterProgr)
{
int aK0 = (aNbIter *aNbIm2Init) / aNbIterProgr;
int aK1 = ((aNbIter+1) *aNbIm2Init) / aNbIterProgr;
for (int aKIm=aK0; aKIm<aK1 ; aKIm++)
{
ElPackHomologue aPack;
cImagH * anIm = aVIms[aKIm];
const std::vector<cLink2Img*> & aVL = anIm->VLink();
int aNbInit=0;
for (int aKL=0 ; aKL<int(aVL.size()) ; aKL++)
{
cLink2Img * aLnK = aVL[aKL];
cImagH * anI2 = aLnK->Dest();
if (anI2->InitLoc())
{
const std::vector<Pt3dr> & anEch = aLnK->EchantP1();
cElHomographie aH = anI2->HF()->HomCur() * aLnK->Hom12();
for (int aKP=0 ; aKP<int(anEch.size()) ; aKP++)
{
const Pt3dr & aP3d = anEch[aKP];
Pt2dr aP1 (aP3d.x,aP3d.y);
double aPds = aP3d.z;
Pt2dr aP2 = aH.Direct(aP1);
aPack.Cple_Add(ElCplePtsHomologues(aP1,aP2,aPds));
}
aNbInit++;
}
}
cElHomographie aNewH(aPack,true);
anIm->HF()->ReinitHom(aNewH);
if (Show(eShowDetail)) std::cout << anIm->Name() << " : " << aNbInit << "\n";
}
for (int aKIm=aK0; aKIm<aK1 ; aKIm++)
{
aVIms[aKIm]->InitLoc() = true;
}
if (Show(eShowDetail)) std::cout << "==============================\n";
}
if (mDoCompensLoc)
{
mSetEq.SetPhaseEquation();
double aSomEr=0;
double aSomP=0;
for (int aKIm1=0 ; aKIm1<int(mIms.size()) ; aKIm1++)
{
cImagH * anI1 = mIms[aKIm1];
anI1->AddViscositty((anI1== mImCAmel) ? aPdsFreezC : aPdsLVMStd);
cElHomographie aCurH1 = anI1->HF()->HomCur();
if (anI1->InitLoc())
{
double aPdsE = aPdsEvol / anI1->PdsEchant();
const std::vector<cLink2Img*> & aVL = anI1->VLink();
for (int aKL=0 ; aKL<int(aVL.size()) ; aKL++)
{
cLink2Img * aLnk = aVL[aKL];
cImagH* anI2 = aLnk->Dest();
cElHomographie aCurH2 = anI2->HF()->HomCur();
cElHomographie aCurH2Inv = aCurH2.Inverse();
if (anI2->InitLoc())
{
double aSomRes = 0;
double aSomCtrl = 0;
cElHomographie aH12 = aLnk->Hom12();
const std::vector<Pt3dr> & anEch = aLnk->EchantP1();
cEqHomogFormelle * anEq = aLnk->EqHF();
int aNbPts = anEch.size();
for (int aKEch = 0 ; aKEch<int(aNbPts) ; aKEch++)
{
Pt3dr aP3d = anEch[aKEch];
Pt2dr aP1(aP3d.x,aP3d.y);
Pt2dr aP2 = aH12.Direct(aP1);
double aPds = aP3d.z * aPdsE;
Pt2dr aRes = anEq->StdAddLiaisonP1P2(aP1,aP2,aPds,false);
Pt2dr aCtrl = aCurH2Inv.Direct(aCurH1.Direct(aP1)) - aP2;
aSomRes += euclid(aRes);
aSomCtrl += euclid(aCtrl);
double anEr = square_euclid(aRes);
aSomEr+= anEr * aPds;
aSomP+= aPds;
}
/*
std::cout << anEq
<< " N12=" << anI1->Name() << " " << anI2->Name()
<< " ; RES = " << aSomRes/aNbPts << " Ctrl=" << aSomCtrl/aNbPts << "\n";
*/
}
}
}
// getchar();
// anI->HF()->SetModeCtrl(cNameSpaceEqF::eHomFigee);
}
if (Show(eShowDetail)) std::cout << "ERR = " << sqrt(aSomEr/aSomP) << "\n";
mSetEq.SolveResetUpdate();
}
}
for (int aKIm1=0 ; aKIm1<int(mIms.size()) ; aKIm1++)
{
cImagH * anI1 = mIms[aKIm1];
anI1->H2ImC() = anI1->HF()->HomCur();
}
double aErrorOut = ErrorSolLoc();
if (Show(eShowGlob))
std::cout << "ERROR OUT " << aErrorOut << " DELTA=" << aErrorOut - aErrorIn << "\n";
}
void cBenchCorrSub::MakeACorrel()
{
REAL aSzVgn = 10.0;
REAL aStep = 0.5;
OptCorrSubPix_Diff<U_INT1> aOCSPD
(
mIm1.mIm,mIm2.mIm,
aSzVgn,aStep,
Pt3dr(-10,-10,-10)
);
OptimTranslationCorrelation<U_INT1> aOpComb
(
0.001, 1.0, 1,
mIm1.mIm,mIm2.mIm,
aSzVgn,aStep
);
TIm2D<REAL4,REAL8> aTDX(mDistX);
TIm2D<REAL4,REAL8> aTDY(mDistY);
INT NbStep = 30;
std::vector<cStat> aStatRef(NbStep);
std::vector<cStat> aStatComb(NbStep);
cStat aStatCombRef;
ElTimer aChrono;
REAL aTimeComb =0;
REAL aTimeDiff =0;
for (INT aKP=0 ; aKP<100 ; aKP++)
{
Pt2dr aP1(Rand(aSzVgn,mSz.x),Rand(aSzVgn,mSz.y));
aOpComb.SetP0Im1(aP1);
aOpComb.optim(Pt2dr(0,0));
Pt2dr aPComb = aP1+aOpComb.param();
Pt2dr aPComb1Rec(aTDX.getr(aPComb),aTDY.getr(aPComb));
aStatCombRef.Add(euclid(aP1,aPComb1Rec));
Pt2dr aP2Opt = aP1;
if (aKP % 10 ==0)
{
aChrono.reinit();
for (INT aCp =0 ; aCp < 10 ; aCp++)
aOpComb.optim(Pt2dr(0,0));
aTimeComb += aChrono.uval();
aChrono.reinit();
for (INT aCp =0 ; aCp < 10 ; aCp++)
aOCSPD.Optim(aP1,aP1);
aTimeDiff += aChrono.uval();
}
for (INT aKOpt =0 ; aKOpt<NbStep ; aKOpt++)
{
Pt3dr aPOpt = aOCSPD.Optim(aP1,aP2Opt);
aP2Opt = Pt2dr(aPOpt.x,aPOpt.y);
Pt2dr aP1Rec(aTDX.getr(aP2Opt),aTDY.getr(aP2Opt));
REAL aDistRef = euclid (aP1,aP1Rec);
REAL aDistComb = euclid (aP2Opt,aPComb);
// cout << aDistRef << " " << aDistComb << " " << "\n";
aStatRef[aKOpt].Add(aDistRef);
aStatComb[aKOpt].Add(aDistComb);
}
}
/*
for (INT aKOpt =0 ; aKOpt<NbStep ; aKOpt++)
{
cout << "Step " << aKOpt
<< " DIST MED " << aStatRef[aKOpt].Med()
<< " / Comb " << aStatComb[aKOpt].Med()
<< "\n";
}
cout << "DMED Comb " << aStatCombRef.Med() << "\n";
cout << "Time "
<< " Diff " << aTimeDiff
<< " Comb " << aTimeComb
<< "\n";
*/
BENCH_ASSERT(aStatComb[NbStep-1].Med() < 1e-1);
}
void Test_DBL()
{
Pt2dr aP0(-10,-20);
Pt2dr aP1(1500,2000);
Pt2di aNb(10,15);
cDistorBilin aDBL1(aP0,aP1,aNb);
Box2dr aBoxRab1 = aDBL1.BoxRab(0.3);
//======================================================
// Verif interpol/extrapol de fon lineaire est exacte
//======================================================
for (int aTime=0 ; aTime<10000 ; aTime++)
{
double aF = pow(2.0,NRrandC()*8);
Pt2dr aPP = Pt2dr(NRrandC(),NRrandC()) * aF;
aDBL1.InitAffine(aF,aPP);
for (int aK=0 ; aK<10; aK++)
{
Pt2dr aP0 = aBoxRab1.RandomlyGenereInside();
Pt2dr aP1 = aDBL1.Direct(aP0);
Pt2dr aQ1 = aPP + aP0 * aF;
double aDist = euclid(aP1,aQ1);
if (aDist>1e-9)
{
ELISE_ASSERT(false,"Test_DBL Affine");
}
}
}
//============================
// Test copy
//============================
for (int aK=0 ; aK<10000 ; aK++)
{
aDBL1.Randomize();
cDistorBilin aDBL2 = aDBL1;
Pt2dr aP0 = aBoxRab1.RandomlyGenereInside();
Pt2dr aP1 = aDBL1.Direct(aP0);
Pt2dr aP2 = aDBL2.Direct(aP0);
double aDist = euclid(aP1,aP2);
ELISE_ASSERT(aDist==0,"Test_DBL dist");
}
//============================
// V_SetScalingTranslate
//============================
for (int aTime=0 ; aTime<10000 ; aTime++)
{
double aF = pow(2.0,NRrandC()*8);
Pt2dr aPP = Pt2dr(NRrandC(),NRrandC()) * aF;
aDBL1.Randomize();
cDistorBilin aDBL2 = aDBL1;
aDBL2.V_SetScalingTranslate(aF,aPP);
Box2dr aBoxRab2 = aDBL2.BoxRab(0.3);
for (int aK=0 ; aK<10; aK++)
{
Pt2dr aP0 = aBoxRab2.RandomlyGenereInside();
Pt2dr aP2 = aDBL2.Direct(aP0);
Pt2dr aP1 = (aDBL1.Direct(aPP+aP0*aF)-aPP) /aF;
double aDist = euclid(aP1 - aP2);
ELISE_ASSERT(aDist<1e-9,"DBL-setScalingTranslate");
}
}
//============================
// Verif Inverse
//============================
for (int aTime=0 ; aTime<100000 ; aTime++)
{
aDBL1.Randomize(0.01);
for (int aK=0 ; aK<10; aK++)
{
Pt2dr aP0 = aBoxRab1.RandomlyGenereInside();
Pt2dr aP1 = aDBL1.Direct(aP0);
Pt2dr aP2 = aDBL1.Inverse(aP1);
double aDist = euclid(aP0 - aP2);
// std::cout << "D= " << aDist << "\n";
ELISE_ASSERT(aDist<1e-5,"DBL-setScalingTranslate");
}
}
/*
*/
std::cout << "DONE Test cDistorBilin\n";
}
void cElHJaPlan3D::Show
(
Video_Win aW,
INT aCoul,
bool ShowDroite,
bool ShowInterEmpr
)
{
if (aCoul >=0)
ELISE_COPY(aW.all_pts(),aCoul,aW.ogray());
Box2dr aBoxW(Pt2dr(0,0),Pt2dr(aW.sz()));
for (INT aK=0; aK<INT(mVInters.size()) ; aK++)
{
cElHJaDroite * aDr =mVInters[aK];
if (aDr)
{
ElSeg3D aSeg = aDr->Droite();
Pt3dr aQ0 = aSeg.P0();
Pt3dr aQ1 = aSeg.P1();
Pt2dr aP0(aQ0.x,aQ0.y);
Pt2dr aP1(aQ1.x,aQ1.y);
Seg2d aS(aP0,aP1);
Seg2d aSC = aS.clipDroite(aBoxW);
if (ShowDroite && (! aSC.empty()))
{
aW.draw_seg(aSC.p0(),aSC.p1(),aW.pdisc()(P8COL::magenta));
}
}
}
tFullSubGrPl aSGrFul;
if (ShowInterEmpr)
{
for (tItSomGrPl itS=mGr->begin(aSGrFul) ; itS.go_on() ; itS++)
{
aW.draw_circle_loc
(
(*itS).attr().Pt(),
4.0,
aW.pdisc()(P8COL::blue)
);
for (tItArcGrPl itA=(*itS).begin(aSGrFul) ; itA.go_on() ; itA++)
{
tSomGrPl &s1 = (*itA).s1();
tSomGrPl &s2 = (*itA).s2();
if (&s1 < &s2)
{
aW.draw_seg
(
s1.attr().Pt(),
s2.attr().Pt(),
aW.pdisc()(P8COL::black)
);
}
}
}
}
// for (INT aK=0 ; aK<INT(mFacOblig.size()) ; aK++)
// mFacOblig[aK]->Show(PI/2.0,P8COL::cyan,false);
}
