test GCP constraints during BundleAdj

micmacIGN · Jul 9, 2024 · 7fe22de · 7fe22de
1 parent 4f3b6f3
commit 7fe22de
Show file tree

Hide file tree

Showing 7 changed files with 60 additions and 4 deletions.
diff --git 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
@@ -167,6 +167,7 @@ class cMMVII_BundleAdj
           const std::vector<cSensorCamPC *> &  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
@@ -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
@@ -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
@@ -5,6 +5,7 @@
 #include "ctopoobs.h"
 #include "../BundleAdjustment/BundleAdjustment.h"
 #include "cMMVII_Appli.h"
+#include <algorithm>
 
 namespace MMVII
 {
@@ -189,6 +190,19 @@ void cBA_Topo::printObs(bool withDetails)
     StdOut() << "Topo sigma0: " << mSigma0 << " (" << nbObs <<  " obs)\n";
 }
 
+std::vector<cTopoObs*> cBA_Topo::GetObsPoint(std::string aPtName) const
+{
+    std::vector<cTopoObs*> 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<tREAL8> & aSet)
 {
     MMVII_INTERNAL_ASSERT_strong(mIsReady,"cBA_Topo is not ready");
@@ -229,7 +243,7 @@ cCalculator<double>*  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
@@ -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<cTopoObs*> GetObsPoint(std::string aPtName) const;
 
     bool mergeUnknowns(cResolSysNonLinear<tREAL8> &aSys); //< if several stations share origin etc.
     void makeConstraints(cResolSysNonLinear<tREAL8> &aSys);
     const std::map<std::string, cTopoPoint> & getAllPts() const { return mAllPts; }
     std::map<std::string, cTopoPoint> & getAllPts() { return mAllPts; }
-    cTopoPoint & getPoint(std::string name);
+    const cTopoPoint & getPoint(std::string name) const;
     cCalculator<double>* getEquation(eTopoObsType tot) const;
     tPtrSysCo getSysCo() const { return mSysCo; }
 

diff --git 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<double> & 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;
 };
 
 /**