diff --git a/MMVII/include/MMVII_MeasuresIm.h b/MMVII/include/MMVII_MeasuresIm.h index 7f63293e7f..4b3f4c4d4f 100755 --- a/MMVII/include/MMVII_MeasuresIm.h +++ b/MMVII/include/MMVII_MeasuresIm.h @@ -230,6 +230,7 @@ class cSetMesImGCP : public cMemCheck /// suppress mMesGCP & mMesIm with no images measure (eventually can give higher threshold) cSetMesImGCP * FilterNonEmptyMeasure(int NbMeasureMin=1) const; + int GetNbImMesForPoint(const std::string & aGCPName) const; const cSetMesPtOf1Im & MesImInitOfName(const std::string &) const; const cMes1GCP & MesGCPOfName(const std::string &) const; diff --git a/MMVII/src/BundleAdjustment/BundleAdjustment.h b/MMVII/src/BundleAdjustment/BundleAdjustment.h index e63155662c..da3d2058e7 100644 --- a/MMVII/src/BundleAdjustment/BundleAdjustment.h +++ b/MMVII/src/BundleAdjustment/BundleAdjustment.h @@ -167,6 +167,7 @@ class cMMVII_BundleAdj const std::vector & VSCPC() const; ///< Accessor // + bool CheckGCPConstraints() const; //< test if free points have enough observations // ========= control object free/frozen =================== void SetParamFrozenCalib(const std::string & aPattern); diff --git a/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp b/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp index 684541145e..bf2b08d915 100644 --- a/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp +++ b/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp @@ -2,6 +2,7 @@ #include "MMVII_util_tpl.h" #include "../Topo/Topo.h" +#include "../Topo/ctopoobs.h" /** \file cAppliBundAdj.cpp @@ -151,6 +152,7 @@ void cMMVII_BundleAdj::OneIteration(tREAL8 aLVM) if (mPhaseAdd) { InitIteration(); + CheckGCPConstraints(); } @@ -259,6 +261,7 @@ void cMMVII_BundleAdj::OneIterationTopoOnly(tREAL8 aLVM, bool verbose) if (mPhaseAdd) { InitIteration(); + CheckGCPConstraints(); } mTopo->SetFrozenAndSharedVars(*mR8_Sys); @@ -479,6 +482,36 @@ void cMMVII_BundleAdj::CompileSharedIntrinsicParams(bool ForAvg) } } + +bool cMMVII_BundleAdj::CheckGCPConstraints() const +{ + for (const auto & aBA_GCP_Ptr : mVGCP) + { + for (const auto & aMesGCP : aBA_GCP_Ptr->mMesGCP->MesGCP()) + { + if (aMesGCP.isFree()) + { + + int aNbImObs = aBA_GCP_Ptr->mMesGCP->GetNbImMesForPoint(aMesGCP.mNamePt); + int aNbTopoElementObs = 0; + if (mTopo) + { + for (auto & obs: mTopo->GetObsPoint(aMesGCP.mNamePt)) + { + aNbTopoElementObs += obs->getMeasures().size(); + } + } + MMVII_INTERNAL_ASSERT_strong(aNbImObs*2+aNbTopoElementObs>=3, + "Not enough observations for point "+ + aMesGCP.mNamePt+": "+ + std::to_string(aNbImObs)+" im + "+ + std::to_string(aNbTopoElementObs)+" topo"); + } + } + } + return true; +} + /* ---------------------------------------- */ /* AddViscosity */ /* ---------------------------------------- */ diff --git a/MMVII/src/Sensors/Measures.cpp b/MMVII/src/Sensors/Measures.cpp index 69907a8763..7083078dd7 100644 --- a/MMVII/src/Sensors/Measures.cpp +++ b/MMVII/src/Sensors/Measures.cpp @@ -309,6 +309,11 @@ void cSetMesImGCP::AsserGCPFinished() const MMVII_UnclasseUsEr("cSetMesImGCP : use with no image file"); } +int cSetMesImGCP::GetNbImMesForPoint(const std::string & aGCPName) const +{ + size_t aKGCP = m2MapPtInt.Obj2I(aGCPName,true); + return mMesImOfPt[aKGCP].VImages().size(); +} cSetMesImGCP * cSetMesImGCP::FilterNonEmptyMeasure(int aNbMeasureMin) const { diff --git a/MMVII/src/Topo/Topo.cpp b/MMVII/src/Topo/Topo.cpp index 59b7e1725d..11d271db02 100644 --- a/MMVII/src/Topo/Topo.cpp +++ b/MMVII/src/Topo/Topo.cpp @@ -5,6 +5,7 @@ #include "ctopoobs.h" #include "../BundleAdjustment/BundleAdjustment.h" #include "cMMVII_Appli.h" +#include namespace MMVII { @@ -189,6 +190,19 @@ void cBA_Topo::printObs(bool withDetails) StdOut() << "Topo sigma0: " << mSigma0 << " (" << nbObs << " obs)\n"; } +std::vector cBA_Topo::GetObsPoint(std::string aPtName) const +{ + std::vector aVectObs; + for (auto &obsSet: mAllObsSets) + for (auto & obs: obsSet->getAllObs()) + { + auto & aVectObsPtNames = obs->getPointNames(); + if (std::find(aVectObsPtNames.begin(), aVectObsPtNames.end(), aPtName) != aVectObsPtNames.end()) + aVectObs.push_back(obs); + } + return aVectObs; +} + void cBA_Topo::AddToSys(cSetInterUK_MultipeObj & aSet) { MMVII_INTERNAL_ASSERT_strong(mIsReady,"cBA_Topo is not ready"); @@ -229,7 +243,7 @@ cCalculator* cBA_Topo::getEquation(eTopoObsType tot) const { } } -cTopoPoint & cBA_Topo::getPoint(std::string name) +const cTopoPoint & cBA_Topo::getPoint(std::string name) const { if (mAllPts.count(name)==0) { diff --git a/MMVII/src/Topo/Topo.h b/MMVII/src/Topo/Topo.h index 62ec90f6fc..7100e8fe14 100644 --- a/MMVII/src/Topo/Topo.h +++ b/MMVII/src/Topo/Topo.h @@ -12,6 +12,7 @@ class cMMVII_BundleAdj; class cPhotogrammetricProject; class cSensorCamPC; class cTopoObsSet; +class cTopoObs; class cTopoPoint; class cBA_GCP; @@ -39,12 +40,13 @@ public : void print(); void printObs(bool withDetails=false); double Sigma0() {return mSigma0;} + std::vector GetObsPoint(std::string aPtName) const; bool mergeUnknowns(cResolSysNonLinear &aSys); //< if several stations share origin etc. void makeConstraints(cResolSysNonLinear &aSys); const std::map & getAllPts() const { return mAllPts; } std::map & getAllPts() { return mAllPts; } - cTopoPoint & getPoint(std::string name); + const cTopoPoint & getPoint(std::string name) const; cCalculator* getEquation(eTopoObsType tot) const; tPtrSysCo getSysCo() const { return mSysCo; } diff --git a/MMVII/src/Topo/ctopoobsset.h b/MMVII/src/Topo/ctopoobsset.h index e7f006fd67..a18a70467e 100644 --- a/MMVII/src/Topo/ctopoobsset.h +++ b/MMVII/src/Topo/ctopoobsset.h @@ -78,7 +78,7 @@ class cTopoObsSetStation : public cTopoObsSet void setOrigin(std::string _OriginName); void PushRotObs(std::vector & aVObs) const; cPt3dr_UK & getRotOmega() { return mRotOmega; } - cTopoPoint * getPtOrigin() const { return mPtOrigin; } + const cTopoPoint * getPtOrigin() const { return mPtOrigin; } tREAL8 getG0() const; const tRot & getRotVert2Instr() const { return mRotVert2Instr; } const tRot & getRotSysCo2Vert() const { return mRotSysCo2Vert; } @@ -95,7 +95,7 @@ class cTopoObsSetStation : public cTopoObsSet tRot mRotVert2Instr; //< the station orientation from local vertical frame cPt3dr_UK mRotOmega; //< the station orientation unknown std::string mOriginName; - cTopoPoint *mPtOrigin; + const cTopoPoint * mPtOrigin; }; /**