diff --git a/MMVII/Doc/Methods/LidarImageRegistration.tex b/MMVII/Doc/Methods/LidarImageRegistration.tex index e81d23f35..4628798aa 100644 --- a/MMVII/Doc/Methods/LidarImageRegistration.tex +++ b/MMVII/Doc/Methods/LidarImageRegistration.tex @@ -394,11 +394,11 @@ \subsection{Correlation formulation} \begin{equation} - N_{CC}(X,Y) = 1-\frac{ D^2(\widehat {X} ,\widehat {Y}}{2} + N_{CC}(X,Y) = 1-\frac{ D^2(\widehat {X} ,\widehat {Y})}{2} \end{equation} -So maximizing $N_{CC}$ is equivalent to mininize $ D^2(\widehat {X} ,\widehat {Y}$. Consequently, -for using $N_{CC}$, we will consider the problem of solving the following equation of observation : +So maximizing $N_{CC}$ is equivalent to mininize $ D^2(\widehat {X} ,\widehat {Y})$. Consequently, +for using $N_{CC}$ as a similarity measure, we will consider the problem of solving the following equation of observation : \begin{equation} \widehat {X} =\widehat {Y} \label{Eq:Cor:Pair:Norm} @@ -406,20 +406,20 @@ \subsection{Correlation formulation} Consider now the case where we have $M$ series $X^j_i$ of $N$ values, $j \in [1,M], i \in [1,N]$ and that we want to modelize that they all are similar using correlation. We could use -equation $\ref{Eq:Cor:Pair:Norm}$ for all pair, but this would be un-elagant and un efficient. +equation $\ref{Eq:Cor:Pair:Norm}$ for all pair, but this would be un-elegant and un-efficient. -Instead of that, we introduce the unknown serie $\chi_i$ which modelise the supposed common value -of all $\widehat X^j_i$. We the proceed this way : +Instead of that, we introduce the unknown serie $\chi = \chi_i, i \in [1,N]$ which modelise the supposed common value +of all $\widehat X^j_i$. We then proceed this way : \begin{itemize} - \item as $\chi$ modelise the common value we impose series verifying equation~\ref{CorrelSig1Avg0} + \item as $\chi$ modelise the common value of normalised series (i.e. verifying equation~\ref{CorrelSig1Avg0}); we impose $\mu(\chi)=0, \sigma(\chi)=1$; \item taking into account \ref{CorrelXXHat}, we introduce the unknowns $A^j,B^j$ that link $\chi$ and $X^j$ with equation~\ref{Corel:AjBj} ; \end{itemize} \begin{equation} - A^j X^j + B = \chi \label{Corel:AjBj} + A^j X^j + B^j = \chi \label{Corel:AjBj} \end{equation} To summerize we try to solve the following system : @@ -434,7 +434,7 @@ \subsection{Correlation formulation} \end{equation} -In this system we have : +where we have : \begin{itemize} \item $2 M + N$ unknowns $A^j,B^j,\chi _i$ @@ -442,7 +442,7 @@ \subsection{Correlation formulation} \end{itemize} -For the initialisation, we can use for example equation \ref{Correl:Chi0} for $\chi$ . +For the initialisation of unknowns, we can use for example equation \ref{Correl:Chi0} for $\chi$ . For initialisation of $A^j, B^j$ we can use $\mu^j,\sigma^j$, or solve by least square the equation ~\ref{Corel:AjBj}. diff --git a/MMVII/include/MMVII_2Include_Tiling.h b/MMVII/include/MMVII_2Include_Tiling.h index bdbaf5842..e93e0aeb9 100755 --- a/MMVII/include/MMVII_2Include_Tiling.h +++ b/MMVII/include/MMVII_2Include_Tiling.h @@ -289,6 +289,31 @@ template class cPointSpInd }; +/** Class for geometrically indexing the lidars (on 2D point) for patches creation , used + to instantiate cTilingIndex +*/ + +template class cTil2DTri3D +{ + public : + static constexpr int TheDim = 2; // Pre-requite for instantite cTilingIndex + typedef cPt2dr tPrimGeom; // Pre-requite for instantite cTilingIndex + typedef const cTriangulation3D * tArgPG; // Pre-requite for instantite cTilingIndex + + /** Pre-requite for instantite cTilingIndex : indicate how we extract geometric primitive from one object */ + + tPrimGeom GetPrimGeom(tArgPG aPtrTri) const {return Proj(ToR(aPtrTri->KthPts(mInd)));} + + cTil2DTri3D(size_t anInd) : mInd(anInd) {} + size_t Ind() const {return mInd;} + + private : + size_t mInd; +}; + + + + /** Class for generating point such that all pairs are at distance > given value */ template class cGeneratePointDiff { @@ -373,5 +398,6 @@ template + }; #endif // _MMVII_Tiling_H_ diff --git a/MMVII/include/MMVII_Geom3D.h b/MMVII/include/MMVII_Geom3D.h index 939100bad..d368b1868 100755 --- a/MMVII/include/MMVII_Geom3D.h +++ b/MMVII/include/MMVII_Geom3D.h @@ -329,12 +329,19 @@ template class cTriangulation3D : public cTriangulation cTriangle TriDevlpt(int aKF,int aNumSom) const; // aNumSom in [0,1,2] cDevBiFaceMesh DoDevBiFace(int aKF1,int aNumSom) const; // aNumSom in [0,1,2] + + cBox2dr Box2D() const; + + void MakePatches(std::list > & ,tREAL8 aDistNeigh,tREAL8 aDistReject,int aSzMin) const; + + private : /// Read/Write in ply format using void PlyInit(const std::string &); void PlyWrite(const std::string &,bool isBinary) const; }; + class cPlane3D { public : diff --git a/MMVII/include/MMVII_SysSurR.h b/MMVII/include/MMVII_SysSurR.h index 7005d4419..7e946e64a 100755 --- a/MMVII/include/MMVII_SysSurR.h +++ b/MMVII/include/MMVII_SysSurR.h @@ -231,6 +231,7 @@ template class cResolSysNonLinear : public cREAL8_RSNL void R_SetCurSol(int aNumV,const tREAL8&) override; ///< tREAL8 Equivalent tLinearSysSR * SysLinear() ; ///< Accessor + const tLinearSysSR * SysLinear() const ; ///< Accessor /// Solve solution, update the current solution, Reset the least square system const tDVect & SolveUpdateReset(const Type & aLVM =0.0) ; @@ -299,6 +300,9 @@ template class cResolSysNonLinear : public cREAL8_RSNL void AddConstr(const tSVect & aVect,const Type & aCste,bool OnlyIfFirstIter=true); void SupressAllConstr(); int GetNbLinearConstraints() const; + + Type VarLastSol() const; ///< Call equiv method of SysLinear + Type VarCurSol() const; ///< Call equiv method of SysLinear private : cResolSysNonLinear(const tRSNL & ) = delete; @@ -514,18 +518,40 @@ template class cLinearOverCstrSys : public cMemCheck virtual void AddCov(const cDenseMatrix &,const cDenseVect& ,const std::vector &aVInd); - // + + Type LVMW(int aK) const; + + Type VarLastSol() const; + Type VarCurSol() const; + + protected : + int mNbVar; + + /// method possibi=ly used by heriting class (sparse will do it) to do it by conversion to a dense vector + void SpecificAddObs_UsingCast2Sparse(const Type& aWeight,const cDenseVect & aCoeff,const Type & aRHS) ; + + void AddWRHS(Type aW,Type aRHS); + + private : + + cDenseVect mLVMW; ///< The Levenberg markad weigthing + // mSumWCoeffRHS are update at PublicAddObs, reseted at PublicReset, used at PublicSolve + cDenseVect mSumWCoeffRHS; ///< accumulate the weighted sum of Coeff * RHS for computing residual + Type mSumWRHS2; ///< accumulate the weighted sum of RHS^2 for computing residual + Type mSumW; ///< accumulate the weighted sum of weight + Type mLastSumW; ///< accumulate the weighted sum of weight + Type mLastSumWRHS2; ///< memorize mSumWRHS2 before reset (see discusion & pb with Schurr) + bool mLastResComp; ///< Has last residual been computed ? + Type mLastResidual; ///< Value of last residual (set when PublicSolve is called) + bool mSchurrWasUsed; ///< Was Schurr complement used ? + + /// Add aPds ( aCoeff .X = aRHS) virtual void SpecificAddObservation(const Type& aWeight,const cDenseVect & aCoeff,const Type & aRHS) = 0; /// Add aPds ( aCoeff .X = aRHS) , version sparse virtual void SpecificAddObservation(const Type& aWeight,const cSparseVect & aCoeff,const Type & aRHS) = 0; - Type LVMW(int aK) const; - protected : - int mNbVar; - cDenseVect mLVMW; // The Levenberg markad weigthing - private : /// "Purge" all accumulated equations virtual void SpecificReset() = 0; /// Compute a solution diff --git a/MMVII/src/BundleAdjustment/BundleAdjustment.h b/MMVII/src/BundleAdjustment/BundleAdjustment.h index 173d2e689..b65f342b5 100644 --- a/MMVII/src/BundleAdjustment/BundleAdjustment.h +++ b/MMVII/src/BundleAdjustment/BundleAdjustment.h @@ -20,30 +20,6 @@ class cBA_GCP; class cBA_Clino; class cBA_BlocRig; -/** Class for representing a Pt of R3 in bundle adj, when it is considered as - * unknown. - * + we have the exact value and uncertainty of the point is covariance is used - * - it add (potentially many) unknowns and then it take more place in memory & time - */ - -/* -template class cPtxdr_UK : public cObjWithUnkowns, - public cMemCheck -{ - public : - typedef cPtxd tPt; - - cPtxdr_UK(const tPt &); - ~cPtxdr_UK(); - void PutUknowsInSetInterval() override; - const tPt & Pt() const ; - private : - cPtxdr_UK(const cPtxdr_UK&) = delete; - tPt mPt; -}; - -typedef cPtxdr_UK<3> cPt3dr_UK ; -*/ /** "Standard" weighting classes, used the following formula * @@ -353,23 +329,39 @@ class cBA_TieP class cData1ImLidPhgr { public : - size_t mKIm; // num of images where the patch is seen - std::vector> mVGr; // pair of radiometry/gradient values in each image for each point of the patch + size_t mKIm; ///< num of images where the patch is seen + std::vector> mVGr; ///< pair of radiometry/gradient, in image, for each point of the patch }; -/** Class for doing the adjsment between Lidar & Photogra, prototype for now */ +/** Class for doing the adjsment between Lidar & Photogra, prototype for now, what will most certainly will need + to evolve is the weighting policy. + */ class cBA_LidarPhotogra { public : + /// constructor, take the global bundle struct + one vector of param cBA_LidarPhotogra(cMMVII_BundleAdj&,const std::vector & aParam); + /// destuctor, free interopaltor, calculator .... ~cBA_LidarPhotogra(); + /// add observation with weigh W void AddObs(tREAL8 aW); private : + /** Add observation for 1 Patch of point */ void Add1Patch(tREAL8 aW,const std::vector & aPatch); + + /// Method for adding observations with radiometric differences as similatity criterion + void AddPatchDifRad(tREAL8 aW,const std::vector & aPatch,const std::vector &aVData) ; + + /// Method for adding observations with Census Coeff as similatity criterion + void AddPatchCensus(tREAL8 aW,const std::vector & aPatch,const std::vector &aVData) ; + + /// Method for adding observations with Normalized Centred Coefficent Correlation as similatity criterion + void AddPatchCorrel(tREAL8 aW,const std::vector & aPatch,const std::vector &aVData) ; + void SetVUkVObs ( const cPt3dr& aPGround, @@ -380,17 +372,17 @@ class cBA_LidarPhotogra ); - cMMVII_BundleAdj& mBA; - eImatchCrit mNumMode; - cTriangulation3D mTri; - cDiffInterpolator1D * mInterp; - cCalculator * mEqLidPhgr; - std::vector mVCam; - std::vector> mVIms; - cWeightAv mLastResidual; - std::list > mLPatches; - bool mPertRad; - size_t mNbPointByPatch; + cMMVII_BundleAdj& mBA; ///< The global bundle adj structure + eImatchCrit mModeSim; ///< type of similarity used + cTriangulation3D mTri; ///< Triangulation, in fact used only for points + cDiffInterpolator1D * mInterp; ///< Interpolator, used to extract Value & Grad of images + cCalculator * mEqLidPhgr; ///< Calculator used for constrain the pose from image obs + std::vector mVCam; ///< Vector of central perspective camera + std::vector> mVIms; ///< Vector of images associated to each cam + cWeightAv mLastResidual; ///< Accumulate the radiometric residual + std::list > mLPatches; ///< set of 3D patches + bool mPertRad; ///< do we pertubate the radiometry (simulation & test) + size_t mNbPointByPatch; ///< (approximate) required number of point /patch }; diff --git a/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp b/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp index 4f8e21687..8f65038bd 100644 --- a/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp +++ b/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp @@ -11,39 +11,22 @@ namespace MMVII to instantiate cTilingIndex */ -template class cTil2DTri3D -{ - public : - static constexpr int TheDim = 2; // Pre-requite for instantite cTilingIndex - typedef cPt2dr tPrimGeom; // Pre-requite for instantite cTilingIndex - typedef cTriangulation3D * tArgPG; // Pre-requite for instantite cTilingIndex - - /** Pre-requite for instantite cTilingIndex : indicate how we extract geometric primitive from one object */ - - tPrimGeom GetPrimGeom(tArgPG aPtrTri) const {return Proj(ToR(aPtrTri->KthPts(mInd)));} - - cTil2DTri3D(size_t anInd) : mInd(anInd) {} - size_t Ind() const {return mInd;} - - private : - size_t mInd; -}; cBA_LidarPhotogra::cBA_LidarPhotogra(cMMVII_BundleAdj& aBA,const std::vector& aParam) : mBA (aBA), // memorize the bundel adj class itself (access to optimizer) - mNumMode (Str2E(aParam.at(0))), // mode of matching (int 4 now) 0 ponct, 1 Census + mModeSim (Str2E(aParam.at(0))), // mode of matching (int 4 now) 0 ponct, 1 Census mTri (aParam.at(1)), // Lidar point themself, stored as a triangulation mInterp (nullptr), // interpolator see bellow mEqLidPhgr (nullptr), // equation of egalisation Lidar/Phgr mPertRad (false), mNbPointByPatch (32) { - if (mNumMode==eImatchCrit::eDifRad) + if (mModeSim==eImatchCrit::eDifRad) mEqLidPhgr = EqEqLidarImPonct (true,1); - else if (mNumMode==eImatchCrit::eCensus) + else if (mModeSim==eImatchCrit::eCensus) mEqLidPhgr = EqEqLidarImCensus(true,1); - else if (mNumMode==eImatchCrit::eCorrel) + else if (mModeSim==eImatchCrit::eCorrel) mEqLidPhgr = EqEqLidarImCorrel(true,1); else { @@ -96,65 +79,20 @@ cBA_LidarPhotogra::cBA_LidarPhotogra(cMMVII_BundleAdj& aBA,const std::vector aBoxObj; // Box of object - for (size_t aKP=0 ; aKP 2d , ToR float -> real - aBoxObj.Add(ToR(Proj(mTri.KthPts(aKP)))); - } - // create the "compiled" box from the dynamix - cBox2dr aBox = aBoxObj.CurBox(); - + // cBox2dr aBox = BoxOfTri(mTri); + cBox2dr aBox = mTri.Box2D(); // estimate the distance for computing patching assuming a uniform distributio, // Pi d^ 2 /NbByP = Surf / NbTot tREAL8 aDistMoy = std::sqrt(mNbPointByPatch *aBox.NbElem()/ (mTri.NbPts()*M_PI)); tREAL8 aDistReject = aDistMoy *1.5; - // indexation of all points - cTiling > aTileAll(aBox,true,mTri.NbPts()/20,&mTri); - for (size_t aKP=0 ; aKP(aKP)); - } - - int aCpt=0; - // indexation of all points selecte as center of patches - cTiling > aTileSelect(aBox,true,mTri.NbPts()/20,&mTri); - // parse all points - for (size_t aKP=0 ; aKP(aKP)); - // extract all the point close enough to the center - auto aLIptr = aTileAll.GetObjAtDist(aPt,aDistMoy); - std::vector aPatch; // the patch itself = index of points - aPatch.push_back(aKP); // add the center at begining - for (const auto aPtrI : aLIptr) - { - if (aPtrI->Ind() !=aKP) // dont add the center twice - { - aPatch.push_back(aPtrI->Ind()); - } - } - // some requirement on minimal size - if (aPatch.size() > 5) - { - aCpt += aPatch.size(); - mLPatches.push_back(aPatch); - } - } - } + mTri.MakePatches(mLPatches,aDistMoy,aDistReject,5); StdOut() << "Patches: DistReject=" << aDistReject - << " NbPts=" << mTri.NbPts() << " => " << aCpt - << " NbPatch=" << mLPatches.size() << " NbAvg => " << aCpt / double(mLPatches.size()) + << " NbPts=" << mTri.NbPts() + << " NbPatch=" << mLPatches.size() << "\n"; } } @@ -168,7 +106,7 @@ cBA_LidarPhotogra::~cBA_LidarPhotogra() void cBA_LidarPhotogra::AddObs(tREAL8 aW) { mLastResidual.Reset(); - if (mNumMode==eImatchCrit::eDifRad) + if (mModeSim==eImatchCrit::eDifRad) { for (size_t aKP=0 ; aKP & aVPatchGr, + const std::vector &aVData + ) +{ + // read the solver now, because was not initialized at creation + cResolSysNonLinear * aSys = mBA.Sys(); -void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector & aVPatchGr) + cWeightAv aWAv; // compute average of image for radiom unknown + for (const auto & aData : aVData) + aWAv.Add(1.0,aData.mVGr.at(0).first); + + cPt3dr aPGround = aVPatchGr.at(0); + std::vector aVTmpAvg{aWAv.Average()}; // vector for initializingz the temporay (here 1 = average) + cSetIORSNL_SameTmp aStrSubst(aVTmpAvg); // structure for handling schurr eliminatio, + // parse the data of the patch + for (const auto & aData : aVData) + { + std::vector aVIndUk{-1}; // first one is a temporary (convention < 0) + std::vector aVObs; + SetVUkVObs (aPGround,&aVIndUk,aVObs,aData,0); + + // accumulate the equation involving the radiom + aSys->R_AddEq2Subst(aStrSubst,mEqLidPhgr,aVIndUk,aVObs,aWeight); + } + // do the substitution & add the equation reduced (Schurr complement) + aSys->R_AddObsWithTmpUK(aStrSubst); +} + +void cBA_LidarPhotogra::AddPatchCensus + ( + tREAL8 aWeight, + const std::vector & aVPatchGr, + const std::vector &aVData + ) +{ + // read the solver now, because was not initialized at creation + cResolSysNonLinear * aSys = mBA.Sys(); + for (size_t aKPt=1; aKPt aAvRatio; // stuct for averaging ratio + for (const auto & aData : aVData) + { + tREAL8 aV0 = aData.mVGr.at(0).first; // radiom of central pixel + tREAL8 aVK = aData.mVGr.at(aKPt).first; // radiom of neighbour + aAvRatio.Add(1.0,NormalisedRatioPos(aV0,aVK)) ; // acumulate the ratio + } + std::vector aVTmpAvg({aAvRatio.Average()}); // vector of value of temporary unknowns + + // -------------- [2] Add the observation -------------------- + cSetIORSNL_SameTmp aStrSubst(aVTmpAvg); // structure for schur complement + for (const auto & aData : aVData) // parse all the images + { + std::vector aVIndUk{-1} ; // indexe of unknown + std::vector aVObs; // observation/context + + SetVUkVObs(aVPatchGr.at(0) ,&aVIndUk,aVObs,aData,0); // add unkown AND observations + SetVUkVObs(aVPatchGr.at(aKPt),nullptr ,aVObs,aData,aKPt); // add ONLY observations + aSys->R_AddEq2Subst(aStrSubst,mEqLidPhgr,aVIndUk,aVObs,aWeight); // add the equation in Schurr structure + } + // add all the equation to the system with Schurr's elimination + aSys->R_AddObsWithTmpUK(aStrSubst); + } +} + +void cBA_LidarPhotogra::AddPatchCorrel + ( + tREAL8 aWeight, + const std::vector & aVPatchGr, + const std::vector &aVData + ) { // read the solver now, because was not initialized at creation cResolSysNonLinear * aSys = mBA.Sys(); + // -------------- [1] Compute the normalized values -------------------- + size_t aNbPt = aVPatchGr.size(); + // vector that will store the normalized value (Avg=0, Sigma=1) + cDenseVect aVMoy(aNbPt,eModeInitImage::eMIA_Null); + + // memorize the radiometries of images as vector + std::vector> aListVRad; + for (const auto & aData : aVData) + { + // change to vecor format + cDenseVect aV(aNbPt); + for (size_t aK=0 ; aK< aNbPt ; aK++) + { + aV(aK) = aData.mVGr.at(aK).first; + } + aListVRad.push_back(aV); + cDenseVect aV01 = NormalizeMoyVar(aV); // noramlize value + aVMoy += aV01; // accumulate in a vector + } + + aVMoy *= 1/ tREAL8(aVData.size()); // make VMoy, average of normalized + aVMoy = NormalizeMoyVar(aVMoy); // re normalized + + // -------------- [2] Intialize the temporary -------------------- + + /* Say we have N points, M images, tempory values will be stored "a la queue leu-leu" as : + R1 .. RN A0 B0 A1 B1 ... AM BM + * where Ri are the unknown radiometry of the normalize patch + * where Aj are the unkonw for tranfering radiom of image j to normalize patch such that + + Ri = Aj Imj(pij) + Bj + + Noting pij the projection of Pi in Imj + */ + + std::vector aVTmp = aVMoy.ToStdVect(); // push first values of normalized patch + size_t aK0Im = aVTmp.size(); + + // push the initial values of Aj Bj + for (const auto & aVRad : aListVRad) + { + auto [A,B] = LstSq_Fit_AxPBEqY(aVRad,aVMoy); // solve Ri = Aj Imj + Bj + aVTmp.push_back(A); // add tmp unknown for Aj + aVTmp.push_back(B); // add tmp unknown for Bj + } + cSetIORSNL_SameTmp aStrSubst(aVTmp); // structure for handling schurr eliminatio, + + // three structure for forcing conservation of normalizattion (Avg,Sigma) for VMoy + std::vector aVIndPt; // indexe of unkown of norm radiom + std::vector aVFixAvg; // vector for forcing average + std::vector aVFixVar; // vector for forcing std dev + + // -------------- [3] Add the equation -------------------- + + + for (int aKPt=0 ; aKPt < (int) aNbPt ; aKPt++) // parse all points + { + int aIndPt = -(1+aKPt); // indexe of point are {-1,-2,....} + aVIndPt.push_back(aIndPt); // accumulat set of global indexe of unknown patch + aVFixAvg.push_back(1.0); // Sum Rk = 0 => all weight = 1 + // S(R+dR) ^ 2 =1 ; S (2 R dR ) = 1 - S(R^2) ; but S(R^2)=1 by construction ... + aVFixVar.push_back(2*aVMoy(aKPt)); + for (int aKIm=0 ; aKIm< (int) aVData.size() ; aKIm++) + { + int aIndIm = -(1+aK0Im+2*aKIm); // compute indexe assumming "a la queue leu-leu" + std::vector aVIndUk{aIndPt,aIndIm,aIndIm-1} ; // indexes of 3 unknown + std::vector aVObs; // vector of observations + SetVUkVObs (aVPatchGr.at(aKPt),&aVIndUk,aVObs,aVData.at(aKIm),aKPt); // read obs & global Uk + aSys->R_AddEq2Subst(aStrSubst,mEqLidPhgr,aVIndUk,aVObs,aWeight); // add equation in tmp struct + } + } + + aStrSubst.AddOneLinearObs(aNbPt,aVIndPt,aVFixAvg,0.0); // force average + aStrSubst.AddOneLinearObs(aNbPt,aVIndPt,aVFixVar,0.0); // force standard dev + + aSys->R_AddObsWithTmpUK(aStrSubst); +} + +void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector & aVPatchGr) +{ std::vector aVData; // for each image where patch is visible will store the data - cWeightAv aWAv; // compute average of image for radiom unknown cComputeStdDev aStdDev; // compute the standard deviation of projected radiometry (indicator) // Parse all the image, we will select the images where all point of a patch are visible @@ -265,7 +354,7 @@ void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector & aV aVData.push_back(aData); // memorize the data for this image tREAL8 aValIm = aData.mVGr.at(0).first; // value of first/central pixel in this image - aWAv.Add(1.0,aValIm); // compute average + // aWAv.Add(1.0,aValIm); // compute average aStdDev.Add(1.0,aValIm); // compute std deviation } @@ -279,127 +368,17 @@ void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector & aV mLastResidual.Add(1.0, (aStdDev.StdDev(1e-5) *aVData.size()) / (aVData.size()-1.0)); - if (mNumMode==eImatchCrit::eDifRad) + if (mModeSim==eImatchCrit::eDifRad) { - cPt3dr aPGround = aVPatchGr.at(0); - std::vector aVTmpAvg{aWAv.Average()}; // vector for initializingz the temporay (here 1 = average) - cSetIORSNL_SameTmp aStrSubst(aVTmpAvg); // structure for handling schurr eliminatio, - // parse the data of the patch - for (const auto & aData : aVData) - { - std::vector aVIndUk{-1}; // first one is a temporary (convention < 0) - std::vector aVObs; - SetVUkVObs (aPGround,&aVIndUk,aVObs,aData,0); - - // accumulate the equation involving the radiom - aSys->R_AddEq2Subst(aStrSubst,mEqLidPhgr,aVIndUk,aVObs,aWeight); - } - // do the substitution & add the equation reduced (Schurr complement) - aSys->R_AddObsWithTmpUK(aStrSubst); + AddPatchDifRad(aWeight,aVPatchGr,aVData); } - else if (mNumMode==eImatchCrit::eCensus) + else if (mModeSim==eImatchCrit::eCensus) { - for (size_t aKPt=1; aKPt aAvRatio; // stuct for averaging ratio - for (const auto & aData : aVData) - { - tREAL8 aV0 = aData.mVGr.at(0).first; // radiom of central pixel - tREAL8 aVK = aData.mVGr.at(aKPt).first; // radiom of neighbour - aAvRatio.Add(1.0,NormalisedRatioPos(aV0,aVK)) ; // acumulate the ratio - } - std::vector aVTmpAvg({aAvRatio.Average()}); // vector of value of temporary unknowns - - // -------------- [2] Add the observation -------------------- - cSetIORSNL_SameTmp aStrSubst(aVTmpAvg); // structure for schur complement - for (const auto & aData : aVData) // parse all the images - { - std::vector aVIndUk{-1} ; // indexe of unknown - std::vector aVObs; // observation/context - - SetVUkVObs(aVPatchGr.at(0) ,&aVIndUk,aVObs,aData,0); // add unkown AND observations - SetVUkVObs(aVPatchGr.at(aKPt),nullptr ,aVObs,aData,aKPt); // add ONLY observations - aSys->R_AddEq2Subst(aStrSubst,mEqLidPhgr,aVIndUk,aVObs,aWeight); // add the equation in Schurr structure - } - // add all the equation to the system with Schurr's elimination - aSys->R_AddObsWithTmpUK(aStrSubst); - } + AddPatchCensus(aWeight,aVPatchGr,aVData); } - else if (mNumMode==eImatchCrit::eCorrel) + else if (mModeSim==eImatchCrit::eCorrel) { - // -------------- [1] Compute the normalized values -------------------- - size_t aNbPt = aVPatchGr.size(); - // vector that will store the normalized value (Avg=0, Sigma=1) - cDenseVect aVMoy(aNbPt,eModeInitImage::eMIA_Null); - - // memorize the radiometries of images as vector - std::vector> aListVRad; - for (const auto & aData : aVData) - { - // change to vecor format - cDenseVect aV(aNbPt); - for (size_t aK=0 ; aK< aNbPt ; aK++) - { - aV(aK) = aData.mVGr.at(aK).first; - } - aListVRad.push_back(aV); - cDenseVect aV01 = NormalizeMoyVar(aV); // noramlize value - aVMoy += aV01; // accumulate in a vector - } - - aVMoy *= 1/ tREAL8(aVData.size()); // make VMoy, average of normalized - aVMoy = NormalizeMoyVar(aVMoy); // re normalized - - // -------------- [2] -------------------- - - /* Say we have N points, M images, tempory values will be stored "a la queue leu-leu" as : - R1 .. RN A0 B0 A1 B1 ... AM BM - * where Ri are the unknown radiometry of the normalize patch - * where Aj are the unkonw for tranfering radiom of image j to normalize patch such that - - Ri = Aj Imj(pij) + Bj - - Noting pij the projection of Pi in Imj - */ - - std::vector aVTmp = aVMoy.ToStdVect(); // push first values of normalized patch - size_t aK0Im = aVTmp.size(); - - // push the initial values of Aj Bj - for (const auto & aVRad : aListVRad) - { - auto [A,B] = LstSq_Fit_AxPBEqY(aVRad,aVMoy); // solve Ri = Aj Imj + Bj - aVTmp.push_back(A); // add tmp unknown for Aj - aVTmp.push_back(B); // add tmp unknown for Bj - } - cSetIORSNL_SameTmp aStrSubst(aVTmp); // structure for handling schurr eliminatio, - // 3 structure for forcing conservation of normalizattion (Avg,Sigma) for VMoy - std::vector aVIndPt; // indexe of unkown of norm radiom - std::vector aVFixAvg; // vector for forcing average - std::vector aVFixVar; // vector for forcing std dev - - for (int aKPt=0 ; aKPt < (int) aNbPt ; aKPt++) - { - int aIndPt = -(1+aKPt); // indexe of point are {-1,-2,....} - aVIndPt.push_back(aIndPt); // accumulat set of global indexe of unknown patch - aVFixAvg.push_back(1.0); // Sum Rk = 0 => all weight = 1 - // S(R+dR) ^ 2 =1 ; S (2 R dR ) = 1 - S(R^2) ; but S(R^2)=1 by construction ... - aVFixVar.push_back(2*aVMoy(aKPt)); - - for (int aKIm=0 ; aKIm< (int) aVData.size() ; aKIm++) - { - int aIndIm = -(1+aK0Im+2*aKIm); // compute indexe assumming "a la queue leu-leu" - std::vector aVIndUk{aIndPt,aIndIm,aIndIm-1} ; // indexes of 3 unknown - std::vector aVObs; // vector of observations - SetVUkVObs (aVPatchGr.at(aKPt),&aVIndUk,aVObs,aVData.at(aKIm),aKPt); // read obs & global Uk - aSys->R_AddEq2Subst(aStrSubst,mEqLidPhgr,aVIndUk,aVObs,aWeight); // add equation in tmp struct - } - } - aStrSubst.AddOneLinearObs(aNbPt,aVIndPt,aVFixAvg,0.0); // force average - aStrSubst.AddOneLinearObs(aNbPt,aVIndPt,aVFixVar,0.0); // force standard dev - - aSys->R_AddObsWithTmpUK(aStrSubst); + AddPatchCorrel(aWeight,aVPatchGr,aVData); } } diff --git a/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp b/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp index 7f4e64a43..dcf867ceb 100644 --- a/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp +++ b/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp @@ -296,7 +296,13 @@ void cMMVII_BundleAdj::OneIteration(tREAL8 aLVM) mNbIter++; if(mVerbose) { - StdOut() << "--------------------------- End Iter" << mNbIter << " ---------------" << std::endl; + StdOut() << "---------------------- " + << " End Iter" << mNbIter + << " StdDevLast=" << std::sqrt(mR8_Sys->VarLastSol()) + << " StdDevCur=" << std::sqrt(mR8_Sys->VarCurSol()) + //<< " VarLast=" << mR8_Sys->VarLastSol() + //<< " ?VarCur?=" << mR8_Sys->VarCurSol() + << " ---------------" << std::endl; } } diff --git a/MMVII/src/Matrix/cLeasSqtAA.cpp b/MMVII/src/Matrix/cLeasSqtAA.cpp index 6f4d1af55..39bf563b3 100755 --- a/MMVII/src/Matrix/cLeasSqtAA.cpp +++ b/MMVII/src/Matrix/cLeasSqtAA.cpp @@ -347,20 +347,55 @@ template cLeasSq * cLeasSq::AllocDenseLstSq(int aNbVar) template cLinearOverCstrSys::cLinearOverCstrSys(int aNbVar) : - mNbVar (aNbVar), - mLVMW (aNbVar,eModeInitImage::eMIA_Null) + mNbVar (aNbVar), + mLVMW (aNbVar,eModeInitImage::eMIA_Null), + mSumWCoeffRHS (aNbVar,eModeInitImage::eMIA_Null), + mSumWRHS2 (0.0), + mSumW (0.0), + mLastSumWRHS2 (0.0), + mLastResComp (false), + mLastResidual (0.0), + mSchurrWasUsed (false) { } +template void cLinearOverCstrSys::AddWRHS(Type aW,Type aRHS) +{ + mSumWRHS2 += aW*Square(aRHS); + mSumW += aW; +} + + template void cLinearOverCstrSys::PublicReset() { SpecificReset(); + mLVMW.DIm().InitNull(); + mSumWCoeffRHS.DIm().InitNull(); + mSumWRHS2 = 0 ; + mSumW = 0 ; + mSchurrWasUsed = false; } template cDenseVect cLinearOverCstrSys::PublicSolve() { - return SpecificSolve(); + cDenseVect aSol = SpecificSolve(); + mLastResidual = mSumWRHS2 - mSumWCoeffRHS.DotProduct(aSol); + mLastSumWRHS2 = mSumWRHS2; + mLastSumW = mSumW; + return aSol; +} + + +template Type cLinearOverCstrSys::VarLastSol() const +{ + return mLastSumWRHS2 / mLastSumW; +} + + +template Type cLinearOverCstrSys::VarCurSol() const +{ + return mLastResidual / mLastSumW; } @@ -407,12 +442,27 @@ template Type cLinearOverCstrSys::ResidualOf1Eq } +template + void cLinearOverCstrSys::SpecificAddObs_UsingCast2Sparse + ( + const Type& aWeight, + const cDenseVect & aCoeff, + const Type & aRHS + ) +{ + SpecificAddObservation(aWeight,cSparseVect(aCoeff),aRHS); +} + + template void cLinearOverCstrSys::PublicAddObservation (const Type& aWeight,const cDenseVect & aCoeff,const Type & aRHS) { // No harm to do this optimization , even if done elsewhere if (aWeight==0) return; + AddWRHS(aWeight,aRHS); + mSumWCoeffRHS.WeightedAddIn(aWeight*aRHS,aCoeff); + SpecificAddObservation(aWeight,aCoeff,aRHS); for (int aKV=0 ; aKV void cLinearOverCstrSys::PublicAddObservation (const // No harm to do this optimization , even if done elsewhere if (aWeight==0) return; + AddWRHS(aWeight,aRHS); + mSumWCoeffRHS.WeightedAddIn(aWeight*aRHS,aCoeff); + SpecificAddObservation(aWeight,aCoeff,aRHS); for (const auto & aPair : aCoeff) mLVMW(aPair.mInd) += aWeight * Square(aPair.mVal); @@ -474,6 +527,7 @@ template void cLinearOverCstrSys::SpecificAddObsWithTmpUK(cons template void cLinearOverCstrSys::PublicAddObsWithTmpUK(const cSetIORSNL_SameTmp& aSetSetEq) { SpecificAddObsWithTmpUK(aSetSetEq); + mSchurrWasUsed = true; for (const auto & aSetEq : aSetSetEq.AllEq()) { @@ -483,14 +537,21 @@ template void cLinearOverCstrSys::PublicAddObsWithTmpUK(const const std::vector & aVDer = aSetEq.mDers.at(aKEq); Type aWeight = aSetEq.WeightOfKthResisual(aKEq); + Type aVal = aSetEq.mVals.at(aKEq); + AddWRHS(aWeight,aVal); + cSparseVect aVNonTmp; + for (size_t aKGlob=0 ; aKGlob::IsIndTmp(aInd)) { - mLVMW(aInd) += aWeight * Square(aVDer.at(aKGlob)); + Type aDer = aVDer.at(aKGlob); + mLVMW(aInd) += aWeight * Square(aDer); + aVNonTmp.AddIV(aInd,aDer); } } + mSumWCoeffRHS.WeightedAddIn(aWeight*(-aVal),aVNonTmp); // Note the minus sign because we have a taylor expansion we need to annulate } } diff --git a/MMVII/src/Matrix/cResolSysNonLinear.cpp b/MMVII/src/Matrix/cResolSysNonLinear.cpp index 475d5d6f4..fdf1304c0 100755 --- a/MMVII/src/Matrix/cResolSysNonLinear.cpp +++ b/MMVII/src/Matrix/cResolSysNonLinear.cpp @@ -199,10 +199,18 @@ template cLinearOverCstrSys * cResolSysNonLinear::SysLi SetPhaseEq(); // cautious, if user requires this access he may modify return mSysLinear; } +template const cLinearOverCstrSys * cResolSysNonLinear::SysLinear() const +{ + return mSysLinear; +} template int cResolSysNonLinear::NbVar() const {return mNbVar;} template int cResolSysNonLinear::R_NbVar() const {return NbVar();} + +template Type cResolSysNonLinear::VarLastSol() const {return mSysLinear->VarLastSol() ;} +template Type cResolSysNonLinear::VarCurSol() const {return mSysLinear->VarCurSol() ;} + // ===== handling of frozen vars ================ template void cResolSysNonLinear::SetFrozenVar(int aK,const Type & aVal) diff --git a/MMVII/src/Matrix/cSparseLeastSq.cpp b/MMVII/src/Matrix/cSparseLeastSq.cpp index 7ee0dcb64..227bc11aa 100755 --- a/MMVII/src/Matrix/cSparseLeastSq.cpp +++ b/MMVII/src/Matrix/cSparseLeastSq.cpp @@ -124,7 +124,9 @@ template void cSMLineTransf::TransfertInTriplet template class cSparseLeasSq : public cLeasSq { public : - /// Here genereate an error, no need to handle dense vector in sparse systems + /** Used to genereate an error, no need to handle dense vector in sparse systems + Now less extremist, implement it by convesrsion to sparse vector, btw no need to be efficient + */ void SpecificAddObservation(const Type& aWeight,const cDenseVect & aCoeff,const Type & aRHS) override; cSparseLeasSq(int aNbVar); }; @@ -133,7 +135,9 @@ template void cSparseLeasSq::SpecificAddObservation (const Type& aW ,const cDenseVect & aDV ,const Type & aVal ) { // call to virtual method, dont know why, compiler dont agre w/o cast - static_cast *>(this)-> SpecificAddObservation(aW,cSparseVect(aDV),aVal); + // static_cast *>(this)-> SpecificAddObservation(aW,cSparseVect(aDV),aVal); + this->SpecificAddObs_UsingCast2Sparse(aW,cSparseVect(aDV),aVal); + } template diff --git a/MMVII/src/Mesh/Ply.cpp b/MMVII/src/Mesh/Ply.cpp index 4be24d389..d1d71f3ad 100644 --- a/MMVII/src/Mesh/Ply.cpp +++ b/MMVII/src/Mesh/Ply.cpp @@ -1,6 +1,9 @@ #include "cMMVII_Appli.h" +#include "MMVII_Geom2D.h" #include "MMVII_Geom3D.h" #include "MMVII_Mappings.h" +#include "MMVII_2Include_Tiling.h" + #define WITH_MMV1_FUNCTION false @@ -2265,6 +2268,70 @@ template void cTriangulation3D::CheckOri2D() StdOut() << " 2D-Bad Orientation " << aNbBadOri << " on " << this->NbFace() << "\n" << std::endl; } +template cBox2dr cTriangulation3D::Box2D() const +{ + // create the bounding box of all points + cTplBoxOfPts aBoxObj; // Box of object + for (size_t aKP=0 ; aKPNbPts() ; aKP++) + { + aBoxObj.Add(ToR(Proj(this->KthPts(aKP)))); + } + // create the "compiled" box from the dynamix + return aBoxObj.CurBox(); +} + +template + void cTriangulation3D::MakePatches + ( + std::list > & aLPatches, + tREAL8 aDistNeigh, + tREAL8 aDistReject, + int aSzMin + ) const +{ + cBox2dr aBox = Box2D(); + + // indexation of all points + cTiling > aTileAll(aBox,true,this->NbPts()/20,this); + for (size_t aKP=0 ; aKPNbPts() ; aKP++) + { + aTileAll.Add(cTil2DTri3D(aKP)); + } + + // int aCpt=0; + // indexation of all points selecte as center of patches + cTiling > aTileSelect(aBox,true,this->NbPts()/20,this); + + // parse all points + for (size_t aKP=0 ; aKPNbPts() ; aKP++) + { + cPt2dr aPt = ToR(Proj(this->KthPts(aKP))); + // if the points is not close to an existing center of patch : create a new patch + if (aTileSelect.GetObjAtDist(aPt,aDistReject).empty()) + { + // Add it in the tiling of select + aTileSelect.Add(cTil2DTri3D(aKP)); + // extract all the point close enough to the center + auto aLIptr = aTileAll.GetObjAtDist(aPt,aDistNeigh); + std::vector aPatch; // the patch itself = index of points + aPatch.push_back(aKP); // add the center at begining + for (const auto aPtrI : aLIptr) + { + if (aPtrI->Ind() !=aKP) // dont add the center twice + { + aPatch.push_back(aPtrI->Ind()); + } + } + // some requirement on minimal size + if ((int)aPatch.size() > aSzMin) + { + // aCpt += aPatch.size(); + aLPatches.push_back(aPatch); + } + } + } +} + /* ********************************************************** */ /* */ /* */