diff --git a/MMVII/include/MMVII_Matrix.h b/MMVII/include/MMVII_Matrix.h index d6b07e7bab..3beabe6e8c 100755 --- a/MMVII/include/MMVII_Matrix.h +++ b/MMVII/include/MMVII_Matrix.h @@ -146,7 +146,6 @@ template class cDenseVect Type AvgElem() const; ///< Avereage of all elements void SetAvg(const Type & anAvg); ///< multiply by a cste to fix the average - // operator -= double DotProduct(const cDenseVect &) const; //== scalar product @@ -187,6 +186,17 @@ template class cDenseVect static Type ASymApproxDistBetweenSubspace(const std::vector &,const std::vector &); tIM mIm; }; + +/// return a vectot with avg=0 and som sq = 1 +cDenseVect NormalizeMoyVar(const cDenseVect & aV0,tREAL8 aEpsilon = 1e-5); + +/// fit the equation B +A Vx = Y, return [A,B] +std::pair LstSq_Fit_AxPBEqY(const cDenseVect & aVX,const cDenseVect & aVY); + +// return A X + B .... +cDenseVect MulAXPB(const cDenseVect & , tREAL8 A,tREAL8 B); + + /* To come, sparse vector, will be vect + vect */ /** a Interface class , derived class will be : @@ -872,8 +882,8 @@ template class cComputeStdDev Type NormalizedVal(const Type &) const; cComputeStdDev Normalize(const Type & Epsilon = 0.0) const; Type StdDev(const Type & Epsilon = 0.0) const; - private : void SelfNormalize(const Type & Epsilon = 0.0); + private : Type mSomW; Type mSomWV; Type mSomWV2; diff --git a/MMVII/include/MMVII_SysSurR.h b/MMVII/include/MMVII_SysSurR.h index 2f15d2f255..7005d4419f 100755 --- a/MMVII/include/MMVII_SysSurR.h +++ b/MMVII/include/MMVII_SysSurR.h @@ -11,8 +11,8 @@ namespace MMVII \brief Classes for linear redundant system */ -template class cInputOutputRSNL; -template class cSetIORSNL_SameTmp; +template class cInputOutputRSNL; // class for comunication linearized constraint with Non Linear System +template class cSetIORSNL_SameTmp; // set of cInputOutputRSNL accumulated before schurr-elimination template class cLinearOverCstrSys ; template class cLeasSq ; template class cLeasSqtAA ; @@ -339,6 +339,10 @@ template class cInputOutputRSNL /// Create Input data w/o temporay cInputOutputRSNL(const tVectInd&,const tStdVect & aVObs); + + /// Create an "object" corresponding to one equation "Coeff . dX = aCste" for indexes of aVInd, where dX is delta/current sol + static cInputOutputRSNL CreatFromLinearObs(Type aW,const tVectInd&,const tStdVect & aVCoeffs,Type aCste); + /// Create Input data with temporary temporay // cInputOutputRSNL(const tVectInd&,const tStdVect &aVTmp,const tStdVect & aVObs); @@ -405,6 +409,8 @@ template class cSetIORSNL_SameTmp /// To be Ok must have at least 1 eq, and number of eq must be >= to unkwnonw void AssertOk() const; + /// Add one equation "Coeff . dX = aCste" for indexes of aVInd, where dX is delta/current sol, use CreatFromLinearObs + void AddOneLinearObs(Type aW,const tVectInd&,const tStdVect & aVCoeffs,Type aCste); /// Number of temporary unkown size_t NbTmpUk() const; const tStdVect & ValTmpUk() const; diff --git a/MMVII/include/MMVII_Tpl_Images.h b/MMVII/include/MMVII_Tpl_Images.h index 5955b27e07..b37b15de38 100755 --- a/MMVII/include/MMVII_Tpl_Images.h +++ b/MMVII/include/MMVII_Tpl_Images.h @@ -234,6 +234,9 @@ template template void operator *=(cDataTypedIm & aI2,const T3 & aV3) {SelfMulImageCsteInPlace(aI2,aV3); } +template + void operator *=(cDenseVect & aI2,const T3 & aV3) {SelfMulImageCsteInPlace(aI2.DIm(),aV3); } + /* template void operator *= (cDenseVect & aV2,const T3 & aV3) { aV2.DIm() *= aV3; } template void operator *= (cDenseMatrix & aV2,const T3 & aV3) { aV2.DIm() *= aV3; } diff --git a/MMVII/include/MMVII_enums.h b/MMVII/include/MMVII_enums.h index 8243dbc542..9d6a436c0f 100755 --- a/MMVII/include/MMVII_enums.h +++ b/MMVII/include/MMVII_enums.h @@ -390,7 +390,8 @@ enum class eModeSSR eSSR_LsqDense, ///< Least square, normal equation, with dense implementation eSSR_LsqNormSparse, ///< Least square, normal equation, with sparse implementation eSSR_LsqSparseGC, ///< Least square, NO normal equation (Conjugate Gradient) , with sparse implementation - eSSR_L1Barrodale ///< L1 minimization using Barrodale-Method + eSSR_L1Barrodale, ///< L1 minimization using Barrodale-Method + eNbVals }; @@ -637,6 +638,7 @@ const std::string & E2Str(const eModeEpipMatch &); const std::string & E2Str(const eModeTestPropCov &); const std::string & E2Str(const eModePaddingEpip &); const std::string & E2Str(const eModeCaracMatch &); +const std::string & E2Str(const eModeSSR &); template Type Str2E(const std::string &,bool WithDef=false); template std::string StrAllVall(); diff --git a/MMVII/src/Bench/BenchMatrix.cpp b/MMVII/src/Bench/BenchMatrix.cpp index e4156a6974..072382286f 100755 --- a/MMVII/src/Bench/BenchMatrix.cpp +++ b/MMVII/src/Bench/BenchMatrix.cpp @@ -1077,6 +1077,22 @@ void BenchDenseMatrix0(cParamExeBench & aParam) { if (! aParam.NewBench("Matrix0")) return; + + // Test NormalizeMoyVar , LstSq_Fit_AxPBEqY , MulAXPB + { + int aNbV = 10; + cDenseVect aV0 = cDenseVect::RanGenerate(aNbV); + cDenseVect aV1 = NormalizeMoyVar(aV0); + MMVII_INTERNAL_ASSERT_bench(std::abs(aV1.AvgElem()) < 1e-5,"BenchDenseMatrix0 NormalizeMoyVar"); + MMVII_INTERNAL_ASSERT_bench(std::abs(aV1.L2Norm()-1) < 1e-5,"BenchDenseMatrix0 NormalizeMoyVar"); + + auto [A,B] = LstSq_Fit_AxPBEqY(aV0,aV1); + + MMVII_INTERNAL_ASSERT_bench(aV1.L2Dist(MulAXPB(aV0,A,B)),"BenchDenseMatrix0 LstSq_Fit_AxPBEqY/MulAXPB"); + // StdOut() << "NNNNNN " << aV1.L2Dist(MulAXPB(aV0,A,B)) << "\n"; + // StdOut() << "NNNNNN " <(); Bench_EigenDecompos(aParam); diff --git a/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp b/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp index d155dbce82..2026c1f2a4 100644 --- a/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp +++ b/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp @@ -1,6 +1,7 @@ #include "BundleAdjustment.h" #include "MMVII_Interpolators.h" #include "MMVII_2Include_Tiling.h" +#include "MMVII_Tpl_Images.h" namespace MMVII { @@ -190,7 +191,6 @@ void cBA_LidarPhotogra::SetVUkVObs } - void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector & aVPatchGr) { // read the solver now, because was not initialized at creation @@ -265,6 +265,25 @@ void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector & aV { // to complete ... } + else if (mNumMode==2) // mode correlation + { + size_t aNbPt = aVPatchGr.size(); + cDenseVect aVMoy(aNbPt,eModeInitImage::eMIA_Null); + std::vector> aListVRad; + for (const auto & aData : aVData) + { + 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); + aVMoy += aV01; + } + + aVMoy *= 1/ tREAL8(aVData.size()); + } } }; diff --git a/MMVII/src/Matrix/BaseMatrixes.cpp b/MMVII/src/Matrix/BaseMatrixes.cpp index 8cbf538bc0..34a7c9fbf3 100755 --- a/MMVII/src/Matrix/BaseMatrixes.cpp +++ b/MMVII/src/Matrix/BaseMatrixes.cpp @@ -424,6 +424,53 @@ template Type cDenseVect::ApproxDistBetweenSubspace(const std return std::max(ASymApproxDistBetweenSubspace(aVV1,aVV2),ASymApproxDistBetweenSubspace(aVV2,aVV1)); } +cDenseVect NormalizeMoyVar(const cDenseVect & aV0,tREAL8 aEpsilon) +{ + size_t aSz = aV0.Sz(); + + cComputeStdDev aStdDev; + for (size_t aK=0 ; aK aRes(aSz); + for (size_t aK=0 ; aK LstSq_Fit_AxPBEqY(const cDenseVect & aVX,const cDenseVect & aVY) +{ + MMVII_INTERNAL_ASSERT_tiny(aVX.Sz()==aVY.Sz(),"Different size in LstSq_Fit_AxPBEqY"); + cLeasSqtAA aLsq(2); + + cDenseVect aCoeff(2); + aCoeff(1) = 1.0; + for (int aK=0 ; aK aSol = aLsq.SpecificSolve(); + + return std::pair(aSol(0),aSol(1)); +} + +cDenseVect MulAXPB(const cDenseVect & aV0, tREAL8 A,tREAL8 B) +{ + size_t aSz = aV0.Sz(); + cDenseVect aRes(aSz); + for (size_t aK=0 ; aK void AddData(const cAuxAr2007 & anAux, cDenseVect & aDV) { diff --git a/MMVII/src/Matrix/cInputOutputRSNL.cpp b/MMVII/src/Matrix/cInputOutputRSNL.cpp index b9bf28cedd..16cb25047e 100755 --- a/MMVII/src/Matrix/cInputOutputRSNL.cpp +++ b/MMVII/src/Matrix/cInputOutputRSNL.cpp @@ -110,6 +110,26 @@ template void cInputOutputRSNL::Show() const //StdOut() << std::endl; } +template + cInputOutputRSNL + cInputOutputRSNL::CreatFromLinearObs + ( + Type aWeight, + const tVectInd& aVecInd, + const tStdVect & aVCoeffs, + Type aCste + ) +{ + MMVII_INTERNAL_ASSERT_tiny((aVecInd.size()==aVCoeffs.size()) ,"Bad size for fix var tmp"); + cInputOutputRSNL aResult(aVecInd,{}); + + aResult.mWeights.push_back(aWeight); + aResult.mDers.push_back(aVCoeffs); + aResult.mVals.push_back(-aCste); + + return aResult; +} + /* ************************************************************ */ @@ -219,6 +239,14 @@ template void cSetIORSNL_SameTmp::AddOneEq(const tIO_OneEq & } } +/* +void TrapMultipleInitializer(tREAL8 a) +{ + TrapMultipleInitializer(1.0); + TrapMultipleInitializer({2.0}); // Compile !! +} +*/ + template void cSetIORSNL_SameTmp::AddFixVarTmp (int aInd,const Type& aVal,const Type& aWeight) { MMVII_INTERNAL_ASSERT_tiny @@ -232,12 +260,25 @@ template void cSetIORSNL_SameTmp::AddFixVarTmp (int aInd,co cInputOutputRSNL aIO({aInd},{}); aIO.mWeights.push_back(aWeight); aIO.mDers.push_back({1.0}); + // Constraint is expressed in "absolute", i.e not as a delta with current solution + // (see AddFixCurVarTmp to be 100% sure of this convention). So we have to express + // it taking into account the current solution as this the way + // + // also note it stored as "A.X + B = 0" as in taylor expansion of function Type aDVal = Val1TmpUk(aInd)-aVal; - aIO.mVals.push_back({aDVal}); + // aIO.mVals.push_back({aDVal}); => ?? , accepted, equivalent to line bellow ... + aIO.mVals.push_back(aDVal ); AddOneEq(aIO); } + +template void cSetIORSNL_SameTmp::AddOneLinearObs(Type aW,const tVectInd& aVInd,const tStdVect & aVCoeffs,Type aCste) +{ + AddOneEq(cInputOutputRSNL::CreatFromLinearObs(aW,aVInd,aVCoeffs,aCste)); +} + + template void cSetIORSNL_SameTmp::AddFixCurVarTmp (int aInd,const Type& aWeight) { AddFixVarTmp(aInd,Val1TmpUk(aInd),aWeight); diff --git a/MMVII/src/Matrix/cResolSysNonLinear.cpp b/MMVII/src/Matrix/cResolSysNonLinear.cpp index 73a40c5005..475d5d6f48 100755 --- a/MMVII/src/Matrix/cResolSysNonLinear.cpp +++ b/MMVII/src/Matrix/cResolSysNonLinear.cpp @@ -544,7 +544,7 @@ template void cResolSysNonLinear::CalcVal MMVII_INTERNAL_ASSERT_tiny(aCalcVal->NbInBuf()==0,"Buff not empty"); // Usefull only to test correcness of DoOneEval - bool TestOneVal = aVIO.size()==1; + bool TestOneVal = (aVIO.size()==1); // Put input data for (const auto & aIO : aVIO) { diff --git a/MMVII/src/Serial/ElemStrToVal.cpp b/MMVII/src/Serial/ElemStrToVal.cpp index 2a9d243e35..fdcc73f6a7 100644 --- a/MMVII/src/Serial/ElemStrToVal.cpp +++ b/MMVII/src/Serial/ElemStrToVal.cpp @@ -457,6 +457,16 @@ template<> cE2Str::tMapE2Str cE2Str::mE2S {eFormatSensor::eDimap_RPC,"Dimap_RPC"} }; +template<> cE2Str::tMapE2Str cE2Str::mE2S + { + {eModeSSR::eSSR_LsqDense , "LsqDense"}, + {eModeSSR::eSSR_LsqNormSparse , "LsqNormSparse"}, + {eModeSSR::eSSR_LsqSparseGC , "LsqSparseGC"}, + {eModeSSR::eSSR_L1Barrodale , "L1Barrodale"} + }; + + + template<> cE2Str::tMapE2Str cE2Str::mE2S { {eTypeSerial::exml,"xml"}, @@ -622,6 +632,7 @@ void BenchEnum(cParamExeBench & aParam) TplBenchEnum(); TplBenchEnum(); TplBenchEnum(); + TplBenchEnum(); aParam.EndBench(); } @@ -1224,6 +1235,7 @@ MACRO_INSTANTITATE_STRIO_ENUM(eTyUnitAngle,"AngleUnit") MACRO_INSTANTITATE_STRIO_ENUM(eTypeSensor,"TypeSensor") MACRO_INSTANTITATE_STRIO_ENUM(eFormatSensor,"FormatSensor") +MACRO_INSTANTITATE_STRIO_ENUM(eModeSSR,"ModeSRR") /* ==================================== */ /* */ diff --git a/MMVII/src/TutoBenchTrianguRSNL/cMainNetwork.cpp b/MMVII/src/TutoBenchTrianguRSNL/cMainNetwork.cpp index 5cb04f558b..7551f678b7 100755 --- a/MMVII/src/TutoBenchTrianguRSNL/cMainNetwork.cpp +++ b/MMVII/src/TutoBenchTrianguRSNL/cMainNetwork.cpp @@ -65,6 +65,7 @@ template cMainNetwork ::cMainNetwork mBoxPts (cBox2dr::Empty()), mWeightSetSchur (aWeightSetSchur) { + } template void cMainNetwork ::PostInit() @@ -390,6 +391,30 @@ template Type cMainNetwork::DoOneIterationCompensation(double std::vector aVObs{ObsDist(aPN1,aPN2)}; // compute observations=target distance // Add eq in aSetIO, using CalcD intantiated with VInd,aVTmp,aVObs mSys->AddEq2Subst(aSetIO,mCalcD,aVIndMixt,aVObs); + + static int aCpt = 0; + aCpt++; + if (aCpt%7==3) + { + /** Test the correctness of aSetIO.AddOneLinearObs, it is added in a context where it is useless, but at least we check that it + * does not perurbates the solution + */ + std::vector aVTheor = Append(aPN1.TheorPt().ToStdVector(),aPN2.TheorPt().ToStdVector()); + std::vector aVCur = Append(aPN1.PCur().ToStdVector(),aPN2.PCur().ToStdVector()); + + std::vector aVCoeff; + tREAL8 aCste = 0; + // 0 = Coeff . (X-Th) = Coeff . (X-XCur + XCur-XTh) => Coeff .dX = Coeff .(XTh-XCur) + for (int aK=0 ; aK<4 ; aK++) + { + Type aCoeff = RandInInterval(0.1,2.0); + aVCoeff.push_back(aCoeff); + aCste += aCoeff * (aVTheor.at(aK)-aVCur.at(aK)); + } + size_t aNbPb = (sizeof(Type)==4) + 2*(mModeSSR==eModeSSR::eSSR_LsqSparseGC); + std::vector aVWeight {100.0,0.1,0.1,0.001}; + aSetIO.AddOneLinearObs(aVWeight.at(aNbPb),aVIndMixt,aVCoeff,aCste); + } } { if (mWeightSetSchur.at(0)>=0) aSetIO.AddFixVarTmp(-1,aPTh1.x(), mWeightSetSchur.at(0)); // soft constraint-x on theoreticall