Skip to content

Commit

Permalink
Uncertainty in bundle
Browse files Browse the repository at this point in the history
  • Loading branch information
deseilligny committed Feb 7, 2025
1 parent 67e5419 commit 5367f2a
Show file tree
Hide file tree
Showing 10 changed files with 190 additions and 144 deletions.
15 changes: 13 additions & 2 deletions MMVII/include/MMVII_SysSurR.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,12 +195,13 @@ template <class Type> 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<int> & aVIndUC2Compute = {}, // list of variable for which we compute variance
const std::vector<cSparseVect<Type>> & 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<Type> NormalMatrix() const; /// accessor, if was computed
Expand All @@ -218,11 +219,13 @@ template <class Type> class cResult_UC_SUR
cDenseVect<Type> VectSol() const; /// Accesor, usefull to avoid re-computation

private:
void Compile();
void Compile( tRSNL *);
void AssertCompiled() const;


bool mCompiled;
bool mAddAllVar;
std::vector<int> mVIndUC2Compute;
tRSNL * mRSNL;
int mDim;
tLinearSysSR * mSysL;
Expand Down Expand Up @@ -1010,6 +1013,8 @@ template <class Type> class cObjWithUnkowns // : public cObjOfMultipleObjUk<Typ
int IndUk0() const; ///< Accessor
int IndUk1() const; ///< Accessor

void SetNameType(const std::string &);
void SetNameIdObj(const std::string &);
protected :
/// defautl constructor, put non init in all vars
void OUK_Reset();
Expand All @@ -1021,6 +1026,12 @@ template <class Type> class cObjWithUnkowns // : public cObjOfMultipleObjUk<Typ
int mNumObj;
int mIndUk0;
int mIndUk1;
/// probably should have existed from the beginnin
std::string mOUK_NameType;
std::string mOUK_IdObj;
static std::string NamesTypeId_NonInit() ;
/// fix with mOUK_Name.. if they have been initiated
void SetNameTypeId(cGetAdrInfoParam<tREAL8> & aGAIP) const;
};

template <class T1,class T2> void ConvertVWD(cInputOutputRSNL<T1> & aIO1 , const cInputOutputRSNL<T2> & aIO2);
Expand Down
5 changes: 4 additions & 1 deletion MMVII/src/Bench/BenchPropagUncert.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,7 +195,8 @@ void cBenchLstSqEstimUncert::DoIt
}

// cDenseMatrix<tREAL8> aMUC = mSys->SysLinear()->V_tAA(); // normal matrix , do it before Reset !!
cResult_UC_SUR<tREAL8> aRSUR(mSys,false,true,aVFreeVar,aVSpCombBlin);
cResult_UC_SUR<tREAL8> * aPtrRSUR = new cResult_UC_SUR<tREAL8>(false,true,aVFreeVar,aVSpCombBlin);
cResult_UC_SUR<tREAL8> aRSUR = *aPtrRSUR;
// aRSUR.SetDoComputeNormalMatrix(true);

tDV aSol = mSys->SolveUpdateReset(0.0,{&aRSUR}); // compute the solution of this config
Expand Down Expand Up @@ -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
Expand Down
23 changes: 21 additions & 2 deletions MMVII/src/BundleAdjustment/BundleAdjustment.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -386,6 +386,16 @@ class cBA_LidarPhotogra
};


struct cBundleBlocNamedVar
{
public :
std::string mType;
std::string mIdObj;
int mIndVar0;
std::vector<std::string> mNamesVar;
std::vector<bool> mActivVar;
};


class cMMVII_BundleAdj
{
Expand Down Expand Up @@ -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<cSensorImage *> & VSIm() const ; ///< Accessor
Expand All @@ -448,7 +458,8 @@ class cMMVII_BundleAdj
void Save_newGCP3D();
void SaveTopo();

void ShowUKNames() ;
void Set_UC_UK(const std::vector<std::string> & aParam);
void ShowUKNames(const std::vector<std::string> & aParam) ;
// Save results of clino bundle adjustment
void SaveClino();
void AddBenchSensor(cSensorCamPC *); // Add sensor, used in Bench Clino
Expand Down Expand Up @@ -532,6 +543,14 @@ class cMMVII_BundleAdj
//
int mNbIter; /// counter of iteration, at least for debug
bool mVerbose; // print residuals

std::vector<cBundleBlocNamedVar> mVBBNamedV;

bool mShow_UC_UK;
bool mCompute_Uncert;
std::vector<std::string> mParam_UC_UK;
std::vector<int> mIndCompUC;
cResult_UC_SUR<tREAL8>* mRUCSUR;
};


Expand Down
17 changes: 9 additions & 8 deletions MMVII/src/BundleAdjustment/cAppliBundAdj.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ class cAppliBundlAdj : public cMMVII_Appli
bool mMeasureAdded ;
std::vector<std::string> mVSharedIP; ///< Vector for shared intrinsic param

bool mBAShowUKNames; ///< Do We Show the names of unknowns
std::vector<std::string> mParamShow_UK_UC;
};

cAppliBundlAdj::cAppliBundlAdj(const std::vector<std::string> & aVArgs,const cSpecMMVII_Appli & aSpec) :
Expand All @@ -110,8 +110,7 @@ cAppliBundlAdj::cAppliBundlAdj(const std::vector<std::string> & aVArgs,const cSp
mGCPFilterAdd (""),
mNbIter (10),
mLVM (0.0),
mMeasureAdded (false),
mBAShowUKNames (false)
mMeasureAdded (false)
{
}

Expand Down Expand Up @@ -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)")
;
}

Expand Down Expand Up @@ -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<mNbIter ; aKIter++)
{
mBA.OneIteration(mLVM);
bool isLastIter = (aKIter==(mNbIter-1)) ;
mBA.OneIteration(mLVM,isLastIter);
}

// ========== [3] Save resulst =============================
Expand All @@ -351,9 +352,9 @@ int cAppliBundlAdj::Exe()
mBA.SaveTopo(); // just for debug for now
mBA.SaveClino();

if (mBAShowUKNames)
if (IsInit(&mParamShow_UK_UC))
{
mBA.ShowUKNames();
mBA.ShowUKNames(mParamShow_UK_UC);
}

return EXIT_SUCCESS;
Expand Down
98 changes: 74 additions & 24 deletions MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
namespace MMVII
{


/* ************************************************************************ */
/* */
/* cStdWeighterResidual */
Expand Down Expand Up @@ -101,7 +102,9 @@ cMMVII_BundleAdj::cMMVII_BundleAdj(cPhotogrammetricProject * aPhp) :
mSigmaViscAngles (-1.0),
mSigmaViscCenter (-1.0),
mNbIter (0),
mVerbose (true)
mVerbose (true),
mShow_UC_UK (false),
mRUCSUR (nullptr)
{
}

Expand All @@ -114,35 +117,38 @@ cMMVII_BundleAdj::~cMMVII_BundleAdj()
delete mBlRig;
delete mTopo;
delete mBlClino;
delete mRUCSUR;
// DeleteAllAndClear(mGCP_UK);
DeleteAllAndClear(mVBA_Lidar);
}

void cMMVII_BundleAdj::ShowUKNames()
void cMMVII_BundleAdj::ShowUKNames(const std::vector<std::string> & aParam)
{
StdOut() << "=================== ShowUKNamesShowUKNames ===============\n";


// StdOut() << "=================== ShowUKNamesShowUKNames "<< aParam << " ===============\n";
cDenseVect<tREAL8> aVUk = mSetIntervUK.GetVUnKnowns() ;
StdOut() << "====== NBUK=" << aVUk.Sz() << "\n";
size_t aKUk=0;
for (size_t aKObj=0 ; aKObj< mSetIntervUK.NumberObject() ; aKObj++)
{
cObjWithUnkowns<tREAL8> & anObj = mSetIntervUK.KthObj(aKObj);

cGetAdrInfoParam<tREAL8> 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<Type> &);

for (const auto & aBBNV : mVBBNamedV)
{
StdOut() << " ************ " << aBBNV.mType << " : " << aBBNV.mIdObj << "\n";
for (size_t aKV=0 ; aKV<aBBNV.mNamesVar.size() ; aKV++)
{
if (aBBNV.mActivVar.at(aKV))
{
int aIndGlob = aBBNV.mIndVar0 + aKV;
StdOut() << " N=" << aBBNV.mNamesVar.at(aKV) << " V=" << aVUk(aBBNV.mIndVar0 + aKV) ;
if (mRUCSUR)
StdOut() << " UC=" << mRUCSUR->UK_VarCovarEstimate(aIndGlob,aIndGlob);
StdOut() << "\n";
}
}
}
StdOut() << "NBUK=" << mR8_Sys->NbVar() << "\n";
//getchar();
// mSetIntervUK
StdOut() << "=================== ShowUKNamesShowUKNames "<< aParam << " ===============\n";
}

void cMMVII_BundleAdj::Set_UC_UK(const std::vector<std::string> & aParam)
{
mShow_UC_UK = true;
mParam_UC_UK = aParam;
}


Expand Down Expand Up @@ -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<bool>::FromStr(GetDef(mParam_UC_UK,3,std::string("1")));

for (size_t aKObj=0 ; aKObj< mSetIntervUK.NumberObject() ; aKObj++)
{
cObjWithUnkowns<tREAL8> & anObj = mSetIntervUK.KthObj(aKObj);

cGetAdrInfoParam<tREAL8> 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 ; aKV<aBBNV.mNamesVar.size() ; aKV++)
{
bool isOk = MatchRegex(aBBNV.mNamesVar[aKV],aPatVar);
aBBNV.mActivVar.push_back(isOk);
if (isOk)
{
aNbOk ++;
mIndCompUC.push_back(aKV+aIndV0);
}
}
if (aNbOk!=0)
mVBBNamedV.push_back(aBBNV);
}
aIndV0 += aBBNV.mNamesVar.size();
}
}
}


void cMMVII_BundleAdj::OneIteration(tREAL8 aLVM)
void cMMVII_BundleAdj::OneIteration(tREAL8 aLVM,bool isLastIter)
{
// if it's first step, alloc ressources
if (mPhaseAdd)
Expand Down Expand Up @@ -290,8 +335,13 @@ void cMMVII_BundleAdj::OneIteration(tREAL8 aLVM)
for (const auto & aLidarPh : mVBA_Lidar )
aLidarPh->AddObs(1.0);

if (mCompute_Uncert && isLastIter)
{
// StdOut() << "mCompute_UncertmCompute_UncertmCompute_Uncert--------------------------------\n";
mRUCSUR = new cResult_UC_SUR<tREAL8>(false,false,mIndCompUC);
}

const auto & aVectSol = mSys->R_SolveUpdateReset(aLVM);
const auto & aVectSol = mR8_Sys->SolveUpdateReset(aLVM,{},{mRUCSUR});
mSetIntervUK.SetVUnKnowns(aVectSol);

mNbIter++;
Expand Down
4 changes: 3 additions & 1 deletion MMVII/src/Instrumental/cBlockCamInit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

Expand Down
28 changes: 27 additions & 1 deletion MMVII/src/Matrix/cObjWithUnkowns.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,11 +98,37 @@ template <class Type> void cGetAdrInfoParam<Type>::PatternSetToVal(const std::st
/* ******************************** */

// put all value to "bull shit"
template <class Type> cObjWithUnkowns<Type>::cObjWithUnkowns()
template <class Type> cObjWithUnkowns<Type>::cObjWithUnkowns() :
mOUK_NameType (NamesTypeId_NonInit()),
mOUK_IdObj (NamesTypeId_NonInit())
{
OUK_Reset();
}

template <class Type> std::string cObjWithUnkowns<Type>::NamesTypeId_NonInit()
{
return MMVII_NONE;
}
template <class Type> void cObjWithUnkowns<Type>::SetNameType(const std::string & aName)
{
mOUK_NameType = aName;
}

template <class Type> void cObjWithUnkowns<Type>::SetNameIdObj(const std::string & aName)
{
mOUK_IdObj = aName;
}

template <class Type> void cObjWithUnkowns<Type>::SetNameTypeId(cGetAdrInfoParam<tREAL8> & aGAIP) const
{
if (mOUK_NameType != NamesTypeId_NonInit())
aGAIP.SetNameType(mOUK_NameType);

if (mOUK_IdObj != NamesTypeId_NonInit())
aGAIP.SetIdObj(mOUK_IdObj);
}



template <class Type>
std::vector<cObjWithUnkowns<Type> *> cObjWithUnkowns<Type>::GetAllUK()
Expand Down
Loading

0 comments on commit 5367f2a

Please sign in to comment.