From 5367f2ab06590d0afc1c9ff667907eca002b2880 Mon Sep 17 00:00:00 2001 From: deseilligny Date: Fri, 7 Feb 2025 20:09:26 +0100 Subject: [PATCH] Uncertainty in bundle --- MMVII/include/MMVII_SysSurR.h | 15 ++- MMVII/src/Bench/BenchPropagUncert.cpp | 5 +- MMVII/src/BundleAdjustment/BundleAdjustment.h | 23 ++++- MMVII/src/BundleAdjustment/cAppliBundAdj.cpp | 17 ++-- .../src/BundleAdjustment/cMMVII_BundleAdj.cpp | 98 ++++++++++++++----- MMVII/src/Instrumental/cBlockCamInit.cpp | 4 +- MMVII/src/Matrix/cObjWithUnkowns.cpp | 28 +++++- MMVII/src/Matrix/cResolSysNonLinear.cpp | 94 +----------------- MMVII/src/Matrix/cResult_UC_SUR.cpp | 48 ++++++--- MMVII/src/Sensors/cSensorCamPC.cpp | 2 + 10 files changed, 190 insertions(+), 144 deletions(-) diff --git a/MMVII/include/MMVII_SysSurR.h b/MMVII/include/MMVII_SysSurR.h index 78aac451a..f0dfc357a 100755 --- a/MMVII/include/MMVII_SysSurR.h +++ b/MMVII/include/MMVII_SysSurR.h @@ -195,12 +195,13 @@ template class cResult_UC_SUR cResult_UC_SUR ( - tRSNL * aRSNL, // the system it will be used with bool initAllVar=false, // do we compute var/covar of all vars bool computNormalM=false, // do we compute normal matrix (useless in fact ...) const std::vector & aVIndUC2Compute = {}, // list of variable for which we compute variance const std::vector> & aVLinearComb = {} // list of linear combination for variance comp ); + ~cResult_UC_SUR(); + Type FUV() const; ///< Accessor to "Unitary Factor" of variance or "sigma0", rather for test cDenseMatrix NormalMatrix() const; /// accessor, if was computed @@ -218,11 +219,13 @@ template class cResult_UC_SUR cDenseVect VectSol() const; /// Accesor, usefull to avoid re-computation private: - void Compile(); + void Compile( tRSNL *); void AssertCompiled() const; bool mCompiled; + bool mAddAllVar; + std::vector mVIndUC2Compute; tRSNL * mRSNL; int mDim; tLinearSysSR * mSysL; @@ -1010,6 +1013,8 @@ template class cObjWithUnkowns // : public cObjOfMultipleObjUk class cObjWithUnkowns // : public cObjOfMultipleObjUk & aGAIP) const; }; template void ConvertVWD(cInputOutputRSNL & aIO1 , const cInputOutputRSNL & aIO2); diff --git a/MMVII/src/Bench/BenchPropagUncert.cpp b/MMVII/src/Bench/BenchPropagUncert.cpp index 78fa11f73..8188cdae1 100755 --- a/MMVII/src/Bench/BenchPropagUncert.cpp +++ b/MMVII/src/Bench/BenchPropagUncert.cpp @@ -195,7 +195,8 @@ void cBenchLstSqEstimUncert::DoIt } // cDenseMatrix aMUC = mSys->SysLinear()->V_tAA(); // normal matrix , do it before Reset !! - cResult_UC_SUR aRSUR(mSys,false,true,aVFreeVar,aVSpCombBlin); + cResult_UC_SUR * aPtrRSUR = new cResult_UC_SUR(false,true,aVFreeVar,aVSpCombBlin); + cResult_UC_SUR aRSUR = *aPtrRSUR; // aRSUR.SetDoComputeNormalMatrix(true); tDV aSol = mSys->SolveUpdateReset(0.0,{&aRSUR}); // compute the solution of this config @@ -253,6 +254,8 @@ void cBenchLstSqEstimUncert::DoIt // [4.5.3] "Economical" way, dont use explicit inverse of normal matrix but solve N * Col12 = X // then Lin12 * X is Lin12 N-1 Col12 aMoyCov12 = aMoyCov12 + (aLinComb12 * aMatNorm.Solve(aColComb12)) * aFUV; + + delete aPtrRSUR; } //-------------------- [5] finally compare the empiricall solution with theory diff --git a/MMVII/src/BundleAdjustment/BundleAdjustment.h b/MMVII/src/BundleAdjustment/BundleAdjustment.h index 0936acf92..b67348d9a 100644 --- a/MMVII/src/BundleAdjustment/BundleAdjustment.h +++ b/MMVII/src/BundleAdjustment/BundleAdjustment.h @@ -386,6 +386,16 @@ class cBA_LidarPhotogra }; +struct cBundleBlocNamedVar +{ + public : + std::string mType; + std::string mIdObj; + int mIndVar0; + std::vector mNamesVar; + std::vector mActivVar; +}; + class cMMVII_BundleAdj { @@ -421,7 +431,7 @@ class cMMVII_BundleAdj void AddMTieP(const std::string & aName,cComputeMergeMulTieP * aMTP,const cStdWeighterResidual & aWIm); /// One iteration : add all measure + constraint + Least Square Solve/Udpate/Init - void OneIteration(tREAL8 aLVM=0.0); + void OneIteration(tREAL8 aLVM=0.0,bool isLastIter=false); void OneIterationTopoOnly(tREAL8 aLVM=0.0, bool verbose=false); //< if no images const std::vector & VSIm() const ; ///< Accessor @@ -448,7 +458,8 @@ class cMMVII_BundleAdj void Save_newGCP3D(); void SaveTopo(); - void ShowUKNames() ; + void Set_UC_UK(const std::vector & aParam); + void ShowUKNames(const std::vector & aParam) ; // Save results of clino bundle adjustment void SaveClino(); void AddBenchSensor(cSensorCamPC *); // Add sensor, used in Bench Clino @@ -532,6 +543,14 @@ class cMMVII_BundleAdj // int mNbIter; /// counter of iteration, at least for debug bool mVerbose; // print residuals + + std::vector mVBBNamedV; + + bool mShow_UC_UK; + bool mCompute_Uncert; + std::vector mParam_UC_UK; + std::vector mIndCompUC; + cResult_UC_SUR* mRUCSUR; }; diff --git a/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp b/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp index 20ed7a860..586e7480c 100644 --- a/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp +++ b/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp @@ -98,7 +98,7 @@ class cAppliBundlAdj : public cMMVII_Appli bool mMeasureAdded ; std::vector mVSharedIP; ///< Vector for shared intrinsic param - bool mBAShowUKNames; ///< Do We Show the names of unknowns + std::vector mParamShow_UK_UC; }; cAppliBundlAdj::cAppliBundlAdj(const std::vector & aVArgs,const cSpecMMVII_Appli & aSpec) : @@ -110,8 +110,7 @@ cAppliBundlAdj::cAppliBundlAdj(const std::vector & aVArgs,const cSp mGCPFilterAdd (""), mNbIter (10), mLVM (0.0), - mMeasureAdded (false), - mBAShowUKNames (false) + mMeasureAdded (false) { } @@ -159,8 +158,7 @@ cCollecSpecArg2007 & cAppliBundlAdj::ArgOpt(cCollecSpecArg2007 & anArgOpt) << AOpt2007(mParamRefOri,"RefOri","Reference orientation [Ori,SimgaTr,SigmaRot?,PatApply?]",{{eTA2007::ISizeV,"[2,4]"}}) << AOpt2007(mVSharedIP,"SharedIP","Shared intrinc parmaters [Pat1Cam,Pat1Par,Pat2Cam...] ",{{eTA2007::ISizeV,"[2,20]"}}) - << AOpt2007(mBAShowUKNames,"ShowUKN","Show names of unknowns (tuning) ",{{eTA2007::HDV},{eTA2007::Tuning}}) - + << AOpt2007(mParamShow_UK_UC,"UC_UK","Param for uncertainty & Show names of unknowns (tuning)") ; } @@ -329,11 +327,14 @@ int cAppliBundlAdj::Exe() MMVII_INTERNAL_ASSERT_User(mMeasureAdded,eTyUEr::eUnClassedError,"Not any measure added"); + if (IsInit(&mParamShow_UK_UC)) + mBA.Set_UC_UK(mParamShow_UK_UC); // ========== [2] Make Iteration ============================= for (int aKIter=0 ; aKIter & aParam) { - StdOut() << "=================== ShowUKNamesShowUKNames ===============\n"; - - + // StdOut() << "=================== ShowUKNamesShowUKNames "<< aParam << " ===============\n"; cDenseVect aVUk = mSetIntervUK.GetVUnKnowns() ; - StdOut() << "====== NBUK=" << aVUk.Sz() << "\n"; - size_t aKUk=0; - for (size_t aKObj=0 ; aKObj< mSetIntervUK.NumberObject() ; aKObj++) - { - cObjWithUnkowns & anObj = mSetIntervUK.KthObj(aKObj); - - cGetAdrInfoParam aGIP (".*",anObj,false); - StdOut() << " ************ " << aGIP.NameType() << " : " << aGIP.IdObj() << "\n"; - for (const auto & aN : aGIP.VNames()) - { - StdOut() << " # " << aN << " : " << aVUk(aKUk) << "\n"; - aKUk++; - } - // virtual void GetAdrInfoParam(cGetAdrInfoParam &); + for (const auto & aBBNV : mVBBNamedV) + { + StdOut() << " ************ " << aBBNV.mType << " : " << aBBNV.mIdObj << "\n"; + for (size_t aKV=0 ; aKV & aParam) +{ + mShow_UC_UK = true; + mParam_UC_UK = aParam; } @@ -174,10 +180,49 @@ void cMMVII_BundleAdj::InitIteration() mSys = mR8_Sys; CompileSharedIntrinsicParams(false); + + if (mShow_UC_UK) + { + size_t aIndV0 = 0; + std::string aPatType = GetDef(mParam_UC_UK,0,std::string(".*")); + std::string aPatName = GetDef(mParam_UC_UK,1,std::string(".*")); + std::string aPatVar = GetDef(mParam_UC_UK,2,std::string(".*")); + mCompute_Uncert = cStrIO::FromStr(GetDef(mParam_UC_UK,3,std::string("1"))); + + for (size_t aKObj=0 ; aKObj< mSetIntervUK.NumberObject() ; aKObj++) + { + cObjWithUnkowns & anObj = mSetIntervUK.KthObj(aKObj); + + cGetAdrInfoParam aGIP (".*",anObj,false); + cBundleBlocNamedVar aBBNV; + aBBNV.mType = aGIP.NameType(); + aBBNV.mIdObj = aGIP.IdObj(); + aBBNV.mIndVar0 = aIndV0; + aBBNV.mNamesVar = aGIP.VNames(); + + if (MatchRegex(aBBNV.mType,aPatType) && MatchRegex(aBBNV.mIdObj,aPatName) ) + { + int aNbOk=0; + for (size_t aKV=0 ; aKVAddObs(1.0); + if (mCompute_Uncert && isLastIter) + { +// StdOut() << "mCompute_UncertmCompute_UncertmCompute_Uncert--------------------------------\n"; + mRUCSUR = new cResult_UC_SUR(false,false,mIndCompUC); + } - const auto & aVectSol = mSys->R_SolveUpdateReset(aLVM); + const auto & aVectSol = mR8_Sys->SolveUpdateReset(aLVM,{},{mRUCSUR}); mSetIntervUK.SetVUnKnowns(aVectSol); mNbIter++; diff --git a/MMVII/src/Instrumental/cBlockCamInit.cpp b/MMVII/src/Instrumental/cBlockCamInit.cpp index 85ad7993c..995d98a70 100644 --- a/MMVII/src/Instrumental/cBlockCamInit.cpp +++ b/MMVII/src/Instrumental/cBlockCamInit.cpp @@ -293,12 +293,14 @@ void cBlocOfCamera::Set4Compute() { mForInit = false; - for (const auto & [aName, aPoseUk] : mData.mMapPoseUKInBloc) + for (auto & [aName, aPoseUk] : mData.mMapPoseUKInBloc) { MMVII_INTERNAL_ASSERT_tiny(IsNull(aPoseUk.Omega()),"cBlocOfCamera::TransfertFromUK Omega not null"); mMapPoseInit[aName] = aPoseUk.Pose(); // we force the creation a new Id in the bloc because later we will not accept new bloc in compute mode mMatBlocSync.NumStringCreate(aName); + aPoseUk.SetNameType("PoseBlockRig"); + aPoseUk.SetNameIdObj(aName); } } diff --git a/MMVII/src/Matrix/cObjWithUnkowns.cpp b/MMVII/src/Matrix/cObjWithUnkowns.cpp index ecbfb0718..43c737843 100755 --- a/MMVII/src/Matrix/cObjWithUnkowns.cpp +++ b/MMVII/src/Matrix/cObjWithUnkowns.cpp @@ -98,11 +98,37 @@ template void cGetAdrInfoParam::PatternSetToVal(const std::st /* ******************************** */ // put all value to "bull shit" -template cObjWithUnkowns::cObjWithUnkowns() +template cObjWithUnkowns::cObjWithUnkowns() : + mOUK_NameType (NamesTypeId_NonInit()), + mOUK_IdObj (NamesTypeId_NonInit()) { OUK_Reset(); } +template std::string cObjWithUnkowns::NamesTypeId_NonInit() +{ + return MMVII_NONE; +} +template void cObjWithUnkowns::SetNameType(const std::string & aName) +{ + mOUK_NameType = aName; +} + +template void cObjWithUnkowns::SetNameIdObj(const std::string & aName) +{ + mOUK_IdObj = aName; +} + +template void cObjWithUnkowns::SetNameTypeId(cGetAdrInfoParam & aGAIP) const +{ + if (mOUK_NameType != NamesTypeId_NonInit()) + aGAIP.SetNameType(mOUK_NameType); + + if (mOUK_IdObj != NamesTypeId_NonInit()) + aGAIP.SetIdObj(mOUK_IdObj); +} + + template std::vector *> cObjWithUnkowns::GetAllUK() diff --git a/MMVII/src/Matrix/cResolSysNonLinear.cpp b/MMVII/src/Matrix/cResolSysNonLinear.cpp index e5c0311ba..da4e2e332 100755 --- a/MMVII/src/Matrix/cResolSysNonLinear.cpp +++ b/MMVII/src/Matrix/cResolSysNonLinear.cpp @@ -86,96 +86,8 @@ void cREAL8_RSNL::SetAllUnShared() mCurMaxEquiv = 0; } -/* ************************************************************ */ -/* */ -/* cResultSUR */ -/* */ -/* ************************************************************ */ - -#if (0) - -template cResult_UC_SUR::cResult_UC_SUR(tRSNL *aRSNL,bool AddAllVar) : - mRSNL (aRSNL), - mDebug (false), - mNormalM_Compute (false), - mUncert_Compute (false), - mSol (1), - mNormalMatrix (1), - mGlobUncertMatrix (1) -{ -} - -template Type cResult_UC_SUR::FUV() const {return mFUV;} - -template void cResult_UC_SUR::SetDoComputeNormalMatrix(bool doIt ) {mNormalM_Compute=doIt;} -template cDenseMatrix cResult_UC_SUR::NormalMatrix() const -{ - MMVII_INTERNAL_ASSERT_tiny(mNormalM_Compute,"NormalMatrix not computed"); - return mNormalMatrix; -} - - - -std::pair IntervalAfter(const std::pair & aInt0, int aSz) -{ - return std::pair (aInt0.second,aInt0.second+aSz); -} - -template void cResult_UC_SUR::Compile() -{ - mDim = mRSNL->NbVar(); - mNbObs = mRSNL->GetCurNbObs(); - // mNbCstr = mRSNL->LinearConstr()->getNbConstraints(); - mRatioDOF = (mNbObs+mNbCstr)/double(mNbObs-(mDim-mNbCstr)) ; - -/* - aRS->mIndSol = std::pair(0,1); - aRS->mIndUC = IntervalAfter(aRS->mIndSol , aRS->mIndexUC_2Compute.size()); - aRS->mIndSparse = IntervalAfter(aRS->mIndUC , aRS->mSparseV_2Compute.size()); - aRS->mIndDense = IntervalAfter(aRS->mIndSparse , aRS->mDenseV_2Compute.size()); - aRS->mNbIndexe = aRS->mIndDense.second; - - - int aNbCol = aRS->mNbIndexe; - - cDenseMatrix aM2Solve(aNbCol,mNbVar); - - aM2Solve.WriteCol(0,mSysLinear->V_tARhs()); - for (int aK = aRS->mIndUC.first ; aKmIndUC.second ; aK++) - { - cDenseVect aVect(mNbVar,eModeInitImage::eMIA_Null); - aVect(aRS->mIndexUC_2Compute.at(aK-aRS->mIndUC.first)) = 1; - aM2Solve.WriteCol(aK,aVect); - } - - - cDenseMatrix aMSol = mSysLinear->tAA_Solve(aM2Solve); - aRS->mSol = aMSol.ReadCol(0) ;// + mCurGlobSol ; - - - aRS->mVarianceCur = mSysLinear->VarOfSol(aRS->mSol); - aRS->mFUV = aRS->mRatioDOF * aRS->mVarianceCur; - - - - if (aRS->mNormalM_Compute) - { - aRS->mNormalMatrix = mSysLinear->V_tAA(); - if (aRS->mUncert_Compute) - { - - } - } - else if (aRS->mUncert_Compute) - { - } -*/ - -} -#endif - /* ************************************************************ */ /* */ @@ -897,7 +809,8 @@ template } #endif for (auto aPtrSur : AfterCstr) - aPtrSur->Compile(); + if (aPtrSur) + aPtrSur->Compile(this); for (int aK=0 ; aK } for (auto aPtrSur : AfterLVM) - aPtrSur->Compile(); + if (aPtrSur) + aPtrSur->Compile(this); mCurGlobSol += mSysLinear->PublicSolve(); // mCurGlobSol += mSysLinear->SparseSolve(); mSysLinear->PublicReset(); diff --git a/MMVII/src/Matrix/cResult_UC_SUR.cpp b/MMVII/src/Matrix/cResult_UC_SUR.cpp index d4faba3bd..481961f85 100755 --- a/MMVII/src/Matrix/cResult_UC_SUR.cpp +++ b/MMVII/src/Matrix/cResult_UC_SUR.cpp @@ -18,16 +18,17 @@ namespace MMVII template cResult_UC_SUR::cResult_UC_SUR ( - tRSNL *aRSNL, bool addAllVar, bool computNormalM, const std::vector & aVIndUC2Compute, const std::vector> & aVLinearCstr ) : mCompiled (false), - mRSNL (aRSNL), - mDim (mRSNL->NbVar()), - mSysL (mRSNL->SysLinear()), + mAddAllVar (addAllVar), + mVIndUC2Compute (aVIndUC2Compute), + mRSNL (nullptr), + mDim (-1), + mSysL (nullptr), mDebug (false), mNormalM_Compute (computNormalM), mVectCombLin (aVLinearCstr), @@ -36,18 +37,13 @@ template mNormalMatrix (1), mGlobUncertMatrix (1) { - if (addAllVar) - { - for (int aK=0; aK cResult_UC_SUR::~cResult_UC_SUR() +{ +} + + template Type cResult_UC_SUR::FUV() const {return mFUV;} // template void cResult_UC_SUR::SetDoComputeNormalMatrix(bool doIt ) {mNormalM_Compute=doIt;} @@ -79,8 +75,30 @@ template Type cResult_UC_SUR::CombLin_VarCovarEstimate(int } -template void cResult_UC_SUR::Compile() +template void cResult_UC_SUR::Compile(tRSNL * aRSNL) { + if (mRSNL==nullptr) + { + mRSNL = aRSNL; + mDim = mRSNL->NbVar(); + mSysL = mRSNL->SysLinear(); + if (mAddAllVar) + { + for (int aK=0; aK::Compile"); + } + + mNbObs = mRSNL->GetCurNbObs(); mNbCstr = mRSNL->GetNbLinearConstraints(); mRatioDOF = (mNbObs+mNbCstr)/double(mNbObs-(mDim-mNbCstr)) ; diff --git a/MMVII/src/Sensors/cSensorCamPC.cpp b/MMVII/src/Sensors/cSensorCamPC.cpp index ca2e6abd5..2eb636af5 100644 --- a/MMVII/src/Sensors/cSensorCamPC.cpp +++ b/MMVII/src/Sensors/cSensorCamPC.cpp @@ -102,6 +102,8 @@ void cPoseWithUK::FillGetAdrInfoParam(cGetAdrInfoParam & aGAIP) aGAIP.TestParam(this, &( mOmega.x()) ,"Wx"); aGAIP.TestParam(this, &( mOmega.y()) ,"Wy"); aGAIP.TestParam(this, &( mOmega.z()) ,"Wz"); + + SetNameTypeId(aGAIP); } void cPoseWithUK::PutUknowsInSetInterval(cSetInterUK_MultipeObj * aSetInterv)