Topo: print and save residuals

micmacIGN · Mar 15, 2024 · 1b5b272 · 1b5b272
1 parent 82c3ca2
commit 1b5b272
Show file tree

Hide file tree

Showing 8 changed files with 43 additions and 5 deletions.
diff --git a/MMVII/include/MMVII_SysSurR.h b/MMVII/include/MMVII_SysSurR.h
@@ -49,7 +49,9 @@ template <class Type> class cResidualWeighterExplicit: public cResidualWeighter<
 
             cResidualWeighterExplicit(bool isSigmas, const tStdVect & aData);
             virtual tStdVect WeightOfResidual(const tStdVect &) const override;
+            const tStdVect & getSigmas() const { return mSigmas; }
             tStdVect & getSigmas() { return mSigmas; }
+            const tStdVect & geWeights() const { return mWeights; }
             tStdVect & geWeights() { return mWeights; }
             int size() const { return mWeights.size(); }
             void AddData(const  cAuxAr2007 & anAuxInit);

diff --git a/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp b/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp
@@ -233,6 +233,7 @@ void cMMVII_BundleAdj::OneIteration(tREAL8 aLVM)
     if (mTopo) // TOPO
     {
         mTopo->AddTopoEquations(*mR8_Sys);
+        mTopo->printObs(true);
     }
 
     const auto & aVectSol = mSys->R_SolveUpdateReset(aLVM);

diff --git a/MMVII/src/Topo/Topo.cpp b/MMVII/src/Topo/Topo.cpp
@@ -148,6 +148,25 @@ void cBA_Topo::print()
         std::cout<<" - "<<obsSet->toString()<<"\n";
 }
 
+void cBA_Topo::printObs(bool withDetails)
+{
+    tREAL8 aAvgResNorm = 0.0;
+    int nbObs =0;
+    for (auto &obsSet: mAllObsSets)
+        for (auto & obs: obsSet->getAllObs())
+        {
+            if (withDetails)
+                StdOut() << obs->toString()<< "\n";
+            for (unsigned int i=0;i<obs->getMeasures().size();++i)
+            {
+                ++nbObs;
+                aAvgResNorm += fabs(obs->getResiduals()[i]) / obs->getWeights().getSigmas()[i];
+            }
+        }
+    aAvgResNorm /= nbObs;
+    StdOut() << "Topo average std residual: " << aAvgResNorm << "\n";
+}
+
 void cBA_Topo::AddToSys(cSetInterUK_MultipeObj<tREAL8> & aSet)
 {
     MMVII_INTERNAL_ASSERT_strong(mIsReady,"cBA_Topo is not ready");
@@ -218,7 +237,7 @@ void cBA_Topo::AddTopoEquations(cResolSysNonLinear<tREAL8> & aSys)
 {
     mSigma0 = 0.0;
     int aNbObs = 0;
-    int aNbUk = 0; // TODO!!!
+    int aNbUk = 0;
     for (auto &obsSet: mAllObsSets)
         for (size_t i=0;i<obsSet->nbObs();++i)
         {
@@ -229,6 +248,7 @@ void cBA_Topo::AddTopoEquations(cResolSysNonLinear<tREAL8> & aSys)
             for (unsigned int i=0; i<obs->getMeasures().size();++i)
             {
                 double residual = equation->ValComp(0,i);
+                obs->getResiduals().at(i) = residual;
 #ifdef VERBOSE_TOPO
                 StdOut() << "  resid: " << residual << " ";
 #endif

diff --git a/MMVII/src/Topo/Topo.h b/MMVII/src/Topo/Topo.h
@@ -35,6 +35,7 @@ public :
     void FromFile(const std::vector<cBA_GCP *> &vGCP, cPhotogrammetricProject *aPhProj);
     void ToFile(const std::string & aName);
     void print();
+    void printObs(bool withDetails=false);
     double  Sigma0() {return mSigma0;}
     std::string & getInFile() { return mInFile;}
 

diff --git a/MMVII/src/Topo/ctopodata.cpp b/MMVII/src/Topo/ctopodata.cpp
@@ -53,6 +53,7 @@ void cTopoObsData::AddData(const  cAuxAr2007 & anAuxInit)
     MMVII::AddData(cAuxAr2007("Pts",anAux),mPtsNames);
     MMVII::AddData(cAuxAr2007("Measures",anAux),mMeasures);
     MMVII::AddData(cAuxAr2007("Sigmas",anAux),mSigmas);
+    AddOptData(anAux,"LastResiduals",mLastResiduals);
 }
 
 void AddData(const cAuxAr2007 & anAux, cTopoObsData & aTopoObs)
@@ -79,7 +80,10 @@ cTopoData::cTopoData(cBA_Topo* aBA_topo)
         aSetData.mType = aSet->mType;
         for (auto & aObs : aSet->mObs)
         {
-            cTopoObsData aObsData = {aObs->mType, aObs->mPtsNames, aObs->mMeasures, aObs->mWeights.getSigmas()};
+            cTopoObsData aObsData = {
+                aObs->mType, aObs->mPtsNames, aObs->mMeasures,
+                aObs->mWeights.getSigmas(), aObs->getResiduals()
+            };
             aSetData.mObs.push_back(aObsData);
         }
         mAllObsSets.push_back(aSetData);

diff --git a/MMVII/src/Topo/ctopodata.h b/MMVII/src/Topo/ctopodata.h
@@ -25,7 +25,7 @@ class cTopoPointData
     bool mIsFree;
     cPtxd<tREAL8, 3> mSigmas;
     std::optional<cPtxd<tREAL8, 2> > mVertDefl;
-    std::optional<cPtxd<tREAL8, 3> > mFinalCoord;
+    std::optional<cPtxd<tREAL8, 3> > mFinalCoord; // just for output
 };
 void AddData(const cAuxAr2007 & anAux, cTopoPointData & aObsSet);
 
@@ -43,6 +43,7 @@ class cTopoObsData
     std::vector<std::string> mPtsNames;
     std::vector<tREAL8> mMeasures;
     std::vector<tREAL8> mSigmas;
+    std::optional<std::vector<tREAL8>> mLastResiduals; // just for output
 };
 
 void AddData(const cAuxAr2007 & anAux, cTopoObsData & aObsSet);

diff --git a/MMVII/src/Topo/ctopoobs.cpp b/MMVII/src/Topo/ctopoobs.cpp
@@ -13,7 +13,8 @@ namespace MMVII
 
 cTopoObs::cTopoObs(cTopoObsSet* set, cBA_Topo *aBA_Topo, eTopoObsType type, const std::vector<std::string> &ptsNames, const std::vector<tREAL8> & measures, const cResidualWeighterExplicit<tREAL8> &aWeights):
     mSet(set), mBA_Topo(aBA_Topo), mType(type),
-    mPtsNames(ptsNames), mMeasures(measures), mWeights(aWeights)
+    mPtsNames(ptsNames), mMeasures(measures), mWeights(aWeights),
+    mLastResiduals(measures.size(), NAN)
 {
     if (!mSet)
     {
@@ -57,9 +58,15 @@ std::string cTopoObs::toString() const
     oss<<"TopoObs "<<E2Str(mType)<<" ";
     for (auto & pt: mPtsNames)
         oss<<pt<<" ";
-    oss<<"values: ";
+    oss<<"val: ";
     for (auto & val: mMeasures)
         oss<<val<<" ";
+    oss<<"sigma: ";
+    for (auto & val: mWeights.getSigmas())
+        oss<<val<<" ";
+    oss<<"res: ";
+    for (auto & val: mLastResiduals)
+        oss<<val<<" ";
     return oss.str();
 }
 

diff --git a/MMVII/src/Topo/ctopoobs.h b/MMVII/src/Topo/ctopoobs.h
@@ -29,6 +29,7 @@ class cTopoObs
     std::vector<int> getIndices(cBA_Topo *aBA_Topo) const;
     std::vector<tREAL8> getVals() const; //< for least squares (with rotation matrix if needed
     std::vector<tREAL8> & getMeasures() { return mMeasures;} //< original measures
+    std::vector<tREAL8> & getResiduals() { return mLastResiduals;} //< last residuals
     cResidualWeighterExplicit<tREAL8>& getWeights();
     const std::string & getPointName(size_t i) const { return mPtsNames.at(i); }
     //std::vector<tREAL8> getResiduals(const cTopoComp *comp) const;
@@ -43,6 +44,7 @@ class cTopoObs
     std::vector<std::string> mPtsNames;
     std::vector<tREAL8> mMeasures;
     cResidualWeighterExplicit<tREAL8> mWeights;
+    std::vector<tREAL8> mLastResiduals;
 };
 
 };