Topo: fix working with multiple GCP sets

MMVII/include/MMVII_Sensor.h

@@ -37,6 +37,7 @@ class  cSIMap_Ground2ImageAndProf ;
 class  cCalculMetaDataProject;
 class  cGlobCalculMetaDataProject;
 class  cBA_Topo;
+class  cBA_GCP;
 
 /**  helper for cPixelDomain, as the cPixelDomain must be serialisable we must separate the
  * minimal data for description, with def contructor from the more "sophisticated" object  */
@@ -549,7 +550,8 @@ class cPhotogrammetricProject
          void LoadGCP(cSetMesImGCP&,const std::string & aPatFiltrFile="",const std::string & aFiltrNameGCP="",
                       const std::string & aFiltrAdditionalInfoGCP="") const;
 	 ///  For reading GCP from folder potentially != of standard input measures, can add missing points from topo obs
-         void LoadGCPFromFolder(const std::string & aFolder,cSetMesImGCP&, cBA_Topo * aBATopoAddPoints=nullptr,
+         void LoadGCPFromFolder(const std::string & aFolder,cSetMesImGCP&,
+                                std::pair<cBA_Topo *, std::vector<cBA_GCP*>*> aTopoAddPointsInfo,
                                 const std::string & aPatFiltrFile="", const std::string & aFiltrNameGCP="",
                                 const std::string & aFiltrAdditionalInfoGCP="" ) const;
           // if SVP && file doesnt exist, do nothing

MMVII/src/BundleAdjustment/cAppliBundAdj.cpp

@@ -64,7 +64,7 @@ class cAppliBundlAdj : public cMMVII_Appli
 
         std::vector<tREAL8>  ConvParamStandard(const std::vector<std::string> &,size_t aSzMin,size_t aSzMax) ;
         /// New Method for multiple GCP : each 
-        void  AddOneSetGCP(const std::vector<std::string> & aParam);
+        void  AddOneSetGCP(const std::vector<std::string> & aParam, bool aDoAddNewTopoPoints); //< Add topo new points only on last gcp set
         void  AddOneSetTieP(const std::vector<std::string> & aParam);
 
 	std::string               mSpecImIn;
@@ -176,7 +176,7 @@ std::vector<tREAL8>  cAppliBundlAdj::ConvParamStandard(const std::vector<std::st
 }
 
 // VParam standar is done from  Folder +  weight of size [2,5]
-void  cAppliBundlAdj::AddOneSetGCP(const std::vector<std::string> & aVParStd)
+void  cAppliBundlAdj::AddOneSetGCP(const std::vector<std::string> & aVParStd, bool aDoAddNewTopoPoints)
 {
     std::string aFolder = aVParStd.at(0);  // folder
     std::vector<tREAL8>  aGCPW = ConvParamStandard(aVParStd,3,6);
@@ -194,8 +194,10 @@ void  cAppliBundlAdj::AddOneSetGCP(const std::vector<std::string> & aVParStd)
 */
 
     //  load the GCP
-    cSetMesImGCP  aFullMesGCP; 
-    mPhProj.LoadGCPFromFolder(aFolder,aFullMesGCP,mBA.getTopo(),"",mGCPFilter,mGCPFilterAdd);
+    cSetMesImGCP  aFullMesGCP;
+    mPhProj.LoadGCPFromFolder(aFolder, aFullMesGCP,
+                              {aDoAddNewTopoPoints?mBA.getTopo():nullptr, aDoAddNewTopoPoints?&mBA.getVGCP():nullptr},
+                              "", mGCPFilter, mGCPFilterAdd);
 
     for (const auto  & aSens : mBA.VSIm())
     {
@@ -281,11 +283,11 @@ int cAppliBundlAdj::Exe()
     {
         std::vector<std::string>  aVParamStdGCP{mPhProj.DPPointsMeasures().DirIn()};
         AppendIn(aVParamStdGCP,mGCPW);
-        AddOneSetGCP(aVParamStdGCP);
+        AddOneSetGCP(aVParamStdGCP, mAddGCPW.empty());
     }
     // Add  the potential suplementary GCP
     for (const auto& aGCP : mAddGCPW)
-        AddOneSetGCP(aGCP);
+        AddOneSetGCP(aGCP, aGCP==mAddGCPW.back());
 
     if (IsInit(&mTiePWeight))
     {

MMVII/src/BundleAdjustment/cAppliTopoAdj.cpp

@@ -122,7 +122,9 @@ void  cAppliTopoAdj::AddOneSetGCP(const std::vector<std::string> & aVParStd)
 
     //  load the GCP
     cSetMesImGCP  aFullMesGCP; 
-    mPhProj.LoadGCPFromFolder(aFolder,aFullMesGCP,mBA.getTopo(),"",mGCPFilter,mGCPFilterAdd);
+    mPhProj.LoadGCPFromFolder(aFolder,aFullMesGCP,
+                              {mBA.getTopo(),&mBA.getVGCP()},
+                              "",mGCPFilter,mGCPFilterAdd);
 
     //here no 2d mes, fake it
     cSetMesPtOf1Im aSetMesIm;

MMVII/src/Sensors/cPhotogrammetricProject.cpp

@@ -765,7 +765,7 @@ void cPhotogrammetricProject::LoadGCP(cSetMesImGCP& aSetMes,const std::string &
 void cPhotogrammetricProject::LoadGCPFromFolder
      (const std::string & aFolder,
           cSetMesImGCP& aSetMes,
-          cBA_Topo * aBATopoAddPoints,
+          std::pair<cBA_Topo *, std::vector<cBA_GCP*>*> aTopoAddPointsInfo,
           const std::string & aArgPatFiltr,
           const std::string & aFiltrNameGCP,
           const std::string & aFiltrAdditionalInfoGCP) const
@@ -779,8 +779,9 @@ void cPhotogrammetricProject::LoadGCPFromFolder
      // Restore initial current orientation
      aDPPM.SetDirIn(aDirInit);
 
-     if (aBATopoAddPoints)
-         aBATopoAddPoints->AddPointsFromDataToGCP(aSetMes);
+     auto [aBATopo, aVectBA_GCP] = aTopoAddPointsInfo;
+     if (aBATopo) // new points have to be added here to benefit from gcp unknown creation
+         aBATopo->AddPointsFromDataToGCP(aSetMes, aVectBA_GCP);
 }
 
 void cPhotogrammetricProject::CpGCPPattern(const std::string & aDirIn,const std::string & aDirOut,const std::string & aArgPatFiltr) const

MMVII/src/Topo/Topo.cpp

@@ -88,7 +88,7 @@ void cBA_Topo::ToFile(const std::string & aName) const
 }
 
 
-void cBA_Topo::AddPointsFromDataToGCP(cSetMesImGCP &aFullMesGCP)
+void cBA_Topo::AddPointsFromDataToGCP(cSetMesImGCP &aFullMesGCP, std::vector<cBA_GCP *> *aVGCP)
 {
     // fill every ObsSet types
     for (auto & aSetData: mAllTopoDataIn.mAllObsSetStations)
@@ -119,7 +119,19 @@ void cBA_Topo::AddPointsFromDataToGCP(cSetMesImGCP &aFullMesGCP)
     for (auto & aPointName: aAllPointsNames)
     {
         bool found = false;
-        for (auto &aMesGCP: aFullMesGCP.MesGCP())
+        if (aVGCP) // search for the point in all existing cBA_GCP
+            for (auto &aBA_GCP: *aVGCP)
+            {
+                for (auto &aMesGCP: aBA_GCP->mMesGCP->MesGCP())
+                {
+                    if (aMesGCP.mNamePt == aPointName)
+                    {
+                       found = true;
+                       break;
+                    }
+                }
+            }
+        for (auto &aMesGCP: aFullMesGCP.MesGCP()) // search in current MesGCP
         {
             if (aMesGCP.mNamePt == aPointName)
             {
@@ -271,7 +283,6 @@ void cBA_Topo::AddTopoEquations(cResolSysNonLinear<tREAL8> & aSys)
     int aNbObs = aSys.GetNbObs();
     int aNbUk = aSys.NbVar() - aSys.GetNbLinearConstraints();
     mSigma0 = sqrt(mSigma0/(aNbObs-aNbUk));
-    //StdOut() << "Sigma0 topo: " << mSigma0 << "\n";
 }
 
 
@@ -290,7 +301,7 @@ void BenchTopoComp1example(const std::pair<cTopoData, cSetMesGCP>& aBenchData, t
 
     cSetMesImGCP aMesGCPtmp;
     aMesGCPtmp.AddMes3D(aBenchData.second);
-    aTopo->AddPointsFromDataToGCP(aMesGCPtmp);
+    aTopo->AddPointsFromDataToGCP(aMesGCPtmp, nullptr);
     //here no 2d mes, fake it
     cSetMesPtOf1Im aSetMesIm;
     aMesGCPtmp.AddMes2D(aSetMesIm);

MMVII/src/Topo/Topo.h

@@ -33,7 +33,7 @@ public :
 
     //  Do the kernel job : add topo constraints to the system
     void AddTopoEquations(cResolSysNonLinear<tREAL8> &);
-    void AddPointsFromDataToGCP(cSetMesImGCP &aFullMesGCP); //< get creates points in gcp from points names in data from mAllTopoDataIn, clear mAllTopoDataIn
+    void AddPointsFromDataToGCP(cSetMesImGCP &aFullMesGCP, std::vector<cBA_GCP*> * aVGCP); //< get creates points in gcp from points names in data from mAllTopoDataIn, clear mAllTopoDataIn
     void FromData(const std::vector<cBA_GCP *> &vGCP, cPhotogrammetricProject *aPhProj); //< get data from mAllTopoDataIn
     void ToFile(const std::string & aName) const;
     void print();

MMVII/src/Topo/ctopoobsset.cpp

@@ -245,6 +245,9 @@ bool cTopoObsSetStation::initialize()
                 G0 = atan2( aPtTo.getPt()->x() - mPtOrigin->getPt()->x(),
                                    aPtTo.getPt()->y() - mPtOrigin->getPt()->y())
                             - obs->getMeasures().at(0);
+#ifdef VERBOSE_TOPO
+                StdOut()<<"G0 hz: "<<*mPtOrigin->getPt()<<" -> "<<*aPtTo.getPt()<<" mes "<<obs->getMeasures().at(0)<<"\n";
+#endif
                 break;
             }
             if (obs->getType() == eTopoObsType::eDX && (!aObsDY || (aObsDY->getPointName(1)==obs->getPointName(1))))
@@ -260,6 +263,9 @@ bool cTopoObsSetStation::initialize()
             G0 = atan2( aPtTo.getPt()->x() - mPtOrigin->getPt()->x(),
                                aPtTo.getPt()->y() - mPtOrigin->getPt()->y())
                         - atan2( aObsDX->getMeasures()[0], aObsDY->getMeasures()[0]);
+#ifdef VERBOSE_TOPO
+            StdOut()<<"G0 dxy: "<<*mPtOrigin->getPt()<<" -> "<<*aPtTo.getPt()<<" mes "<<aObsDX->getMeasures()[0]<<" "<<aObsDY->getMeasures()[0]<<"\n";
+#endif
         }
         if (std::isfinite(G0))
         {

MMVII/src/Topo/ctopopoint.cpp

@@ -29,7 +29,7 @@ cTopoPoint::cTopoPoint() :
 void cTopoPoint::findUK(const std::vector<cBA_GCP *> & vGCP, cPhotogrammetricProject *aPhProj, const cPt3dr & aCoordIfPureTopo)
 {
 #ifdef VERBOSE_TOPO
-    StdOut() << "findOrMakeUK "<<mName<<": ";
+    StdOut() << "findUK "<<mName<<": ";
 #endif
     MMVII_INTERNAL_ASSERT_strong(!isReady(), "double cTopoPoint::findOrMakeUK for point "+mName);
 
@@ -46,7 +46,7 @@ void cTopoPoint::findUK(const std::vector<cBA_GCP *> & vGCP, cPhotogrammetricPro
                     mPt = &gcp->mGCP_UK.at(i)->Pt(); //< use existing unknown if available
                     mInitCoord = *mPt;
     #ifdef VERBOSE_TOPO
-                    StdOut() << "is a GCP with existing unknowns\n";
+                    StdOut() << "is a GCP with existing unknowns: "<<*mPt<<" "<<mUK<<"\n";
     #endif
                     return;
                 } else {