In adding correl in Lidar/Phgr

MMVII/include/MMVII_Matrix.h

@@ -146,7 +146,6 @@ template <class Type> 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 Type> class  cDenseVect
 	static  Type ASymApproxDistBetweenSubspace(const std::vector<tDV>  &,const std::vector<tDV>  &);
         tIM mIm;
 };
+
+/// return a vectot with avg=0 and som sq = 1
+cDenseVect<tREAL8> NormalizeMoyVar(const cDenseVect<tREAL8> & aV0,tREAL8 aEpsilon = 1e-5);
+
+///  fit the equation B +A Vx  = Y, return  [A,B]
+std::pair<tREAL8,tREAL8> LstSq_Fit_AxPBEqY(const cDenseVect<tREAL8> & aVX,const cDenseVect<tREAL8> & aVY);
+
+// return A X + B ....
+cDenseVect<tREAL8> MulAXPB(const cDenseVect<tREAL8> & , tREAL8 A,tREAL8 B);
+
+
 /* To come, sparse vector, will be vect<int> + vect<double> */
 
 /** a Interface class , derived class will be :
@@ -872,8 +882,8 @@ template <class Type>  class cComputeStdDev
          Type  NormalizedVal(const Type &) const;
          cComputeStdDev<Type>  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; 

MMVII/include/MMVII_SysSurR.h

@@ -11,8 +11,8 @@ namespace MMVII
     \brief Classes for linear redundant system
 */
 
-template <class Type> class  cInputOutputRSNL;
-template <class Type> class  cSetIORSNL_SameTmp;
+template <class Type> class  cInputOutputRSNL; // class for comunication linearized constraint with Non Linear System
+template <class Type> class  cSetIORSNL_SameTmp; // set of cInputOutputRSNL accumulated before schurr-elimination
 template <class Type> class  cLinearOverCstrSys  ;
 template <class Type> class  cLeasSq ;
 template <class Type> class  cLeasSqtAA ;
@@ -339,6 +339,10 @@ template <class Type> 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<Type> 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 Type> 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;

MMVII/include/MMVII_Tpl_Images.h

@@ -234,6 +234,9 @@ template<class T2,class T3,int Dim>
 template<class T2,class T3,int Dim>  
    void operator *=(cDataTypedIm<T2,Dim> & aI2,const T3 & aV3) {SelfMulImageCsteInPlace(aI2,aV3); }
 
+template<class T2,class T3>  
+   void operator *=(cDenseVect<T2> & aI2,const T3 & aV3) {SelfMulImageCsteInPlace(aI2.DIm(),aV3); }
+
 /*
 template<class T2,class T3> void operator *= (cDenseVect<T2> & aV2,const T3 & aV3) { aV2.DIm() *= aV3; }
 template<class T2,class T3> void operator *= (cDenseMatrix<T2> & aV2,const T3 & aV3) { aV2.DIm() *= aV3; }

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 <class Type> Type  Str2E(const std::string &,bool WithDef=false); 
 template <class Type> std::string   StrAllVall();

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<tREAL8> aV0 =  cDenseVect<tREAL8>::RanGenerate(aNbV);
+        cDenseVect<tREAL8> 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 " <<aV1.AvgElem() << " " << aV1.L2Norm() << "\n";
+    };
+
     BenchProj<tREAL8>();
 
     Bench_EigenDecompos(aParam);

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<cPt3dr> & aVPatchGr)
 {
      // read the solver now, because was not initialized at creation 
@@ -265,6 +265,25 @@ void  cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector<cPt3dr> & aV
      {
             // to complete ...
      }
+     else if (mNumMode==2)  // mode correlation
+     {
+         size_t aNbPt = aVPatchGr.size();
+         cDenseVect<tREAL8>  aVMoy(aNbPt,eModeInitImage::eMIA_Null);
+	 std::vector<cDenseVect<tREAL8>>  aListVRad;
+         for (const auto & aData : aVData)
+         {
+              cDenseVect<tREAL8> aV(aNbPt);
+              for (size_t aK=0 ; aK< aNbPt ; aK++)
+              {
+                  aV(aK)  = aData.mVGr.at(aK).first;
+              }
+	      aListVRad.push_back(aV);
+              cDenseVect<tREAL8> aV01 = NormalizeMoyVar(aV);
+	      aVMoy += aV01;
+         }
+
+	 aVMoy *=  1/ tREAL8(aVData.size());
+     }
 }
 
 };

MMVII/src/Matrix/BaseMatrixes.cpp

@@ -424,6 +424,53 @@ template <class Type> Type cDenseVect<Type>::ApproxDistBetweenSubspace(const std
 	return std::max(ASymApproxDistBetweenSubspace(aVV1,aVV2),ASymApproxDistBetweenSubspace(aVV2,aVV1));
 }
 
+cDenseVect<tREAL8> NormalizeMoyVar(const cDenseVect<tREAL8> & aV0,tREAL8 aEpsilon)
+{
+    size_t aSz = aV0.Sz();
+
+    cComputeStdDev<tREAL8>  aStdDev;
+    for (size_t aK=0 ; aK<aSz ; aK++)
+        aStdDev.Add(1.0,aV0(aK));
+    aStdDev.SelfNormalize(aEpsilon);
+
+    tREAL8 aSqrtNb = std::sqrt(aSz);
+    cDenseVect<tREAL8> aRes(aSz);
+    for (size_t aK=0 ; aK<aSz ; aK++)
+        aRes(aK) = aStdDev.NormalizedVal(aV0(aK)) / aSqrtNb ;
+
+    return aRes;
+}
+
+std::pair<tREAL8,tREAL8> LstSq_Fit_AxPBEqY(const cDenseVect<tREAL8> & aVX,const cDenseVect<tREAL8> & aVY)
+{
+    MMVII_INTERNAL_ASSERT_tiny(aVX.Sz()==aVY.Sz(),"Different size in LstSq_Fit_AxPBEqY");
+    cLeasSqtAA<tREAL8> aLsq(2);
+
+    cDenseVect<tREAL8> aCoeff(2);
+    aCoeff(1) = 1.0;
+    for (int aK=0 ; aK<aVX.Sz() ; aK++)
+    {
+        aCoeff(0) = aVX(aK);
+	aLsq.PublicAddObservation(1.0,aCoeff, aVY(aK));
+    }
+
+     cDenseVect<tREAL8> aSol = aLsq.SpecificSolve();
+
+     return std::pair<tREAL8,tREAL8>(aSol(0),aSol(1));
+}
+
+cDenseVect<tREAL8> MulAXPB(const cDenseVect<tREAL8> & aV0, tREAL8 A,tREAL8 B)
+{
+    size_t aSz = aV0.Sz();
+    cDenseVect<tREAL8> aRes(aSz);
+    for (size_t aK=0 ; aK<aSz ; aK++)
+        aRes(aK) = A * aV0(aK) + B;
+
+    return aRes;
+}
+
+
+
 /*
 template <class Type> void AddData(const  cAuxAr2007 & anAux, cDenseVect<Type> & aDV)
 {

MMVII/src/Matrix/cInputOutputRSNL.cpp

@@ -110,6 +110,26 @@ template <class Type> void cInputOutputRSNL<Type>::Show() const
     //StdOut()  << std::endl;
 }
 
+template <class Type>  
+    cInputOutputRSNL<Type>   
+       cInputOutputRSNL<Type>::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<Type> aResult(aVecInd,{});
+
+    aResult.mWeights.push_back(aWeight);
+    aResult.mDers.push_back(aVCoeffs);
+    aResult.mVals.push_back(-aCste);
+
+    return aResult;
+}
+
 
 
 /* ************************************************************ */
@@ -219,6 +239,14 @@ template <class Type> void cSetIORSNL_SameTmp<Type>::AddOneEq(const tIO_OneEq &
     }
 }
 
+/*
+void TrapMultipleInitializer(tREAL8 a)
+{
+	TrapMultipleInitializer(1.0);
+	TrapMultipleInitializer({2.0});  // Compile !!
+}
+*/
+
 template <class Type> void   cSetIORSNL_SameTmp<Type>::AddFixVarTmp (int aInd,const Type& aVal,const Type& aWeight)
 {
      MMVII_INTERNAL_ASSERT_tiny
@@ -232,12 +260,25 @@ template <class Type> void   cSetIORSNL_SameTmp<Type>::AddFixVarTmp (int aInd,co
      cInputOutputRSNL<Type> 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 <class Type> void   cSetIORSNL_SameTmp<Type>::AddOneLinearObs(Type aW,const tVectInd& aVInd,const tStdVect & aVCoeffs,Type aCste)
+{
+   AddOneEq(cInputOutputRSNL<Type>::CreatFromLinearObs(aW,aVInd,aVCoeffs,aCste));
+}
+
+
 template <class Type> void   cSetIORSNL_SameTmp<Type>::AddFixCurVarTmp (int aInd,const Type& aWeight)
 {
      AddFixVarTmp(aInd,Val1TmpUk(aInd),aWeight); 

MMVII/src/Matrix/cResolSysNonLinear.cpp

@@ -544,7 +544,7 @@ template <class Type> void   cResolSysNonLinear<Type>::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)
       {

MMVII/src/Serial/ElemStrToVal.cpp

@@ -457,6 +457,16 @@ template<> cE2Str<eFormatSensor>::tMapE2Str cE2Str<eFormatSensor>::mE2S
                 {eFormatSensor::eDimap_RPC,"Dimap_RPC"}
 	   };
 
+template<> cE2Str<eModeSSR>::tMapE2Str cE2Str<eModeSSR>::mE2S
+           {
+                {eModeSSR::eSSR_LsqDense      , "LsqDense"},
+                {eModeSSR::eSSR_LsqNormSparse , "LsqNormSparse"},
+                {eModeSSR::eSSR_LsqSparseGC   , "LsqSparseGC"},
+                {eModeSSR::eSSR_L1Barrodale   , "L1Barrodale"}
+	   };
+
+
+
 template<> cE2Str<eTypeSerial>::tMapE2Str cE2Str<eTypeSerial>::mE2S
            {
                 {eTypeSerial::exml,"xml"},
@@ -622,6 +632,7 @@ void BenchEnum(cParamExeBench & aParam)
     TplBenchEnum<eMTDIm>();
     TplBenchEnum<eTypeSensor>();
     TplBenchEnum<eFormatSensor>();
+    TplBenchEnum<eModeSSR>();
 
     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")
 
 /* ==================================== */
 /*                                      */

MMVII/src/TutoBenchTrianguRSNL/cMainNetwork.cpp

@@ -65,6 +65,7 @@ template <class Type> cMainNetwork <Type>::cMainNetwork
     mBoxPts     (cBox2dr::Empty()),
     mWeightSetSchur  (aWeightSetSchur)
 {
+
 }
 
 template <class Type> void cMainNetwork <Type>::PostInit()
@@ -390,6 +391,30 @@ template <class Type> Type cMainNetwork<Type>::DoOneIterationCompensation(double
                 std::vector<Type> 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<Type> aVTheor = Append(aPN1.TheorPt().ToStdVector(),aPN2.TheorPt().ToStdVector());
+		    std::vector<Type> aVCur = Append(aPN1.PCur().ToStdVector(),aPN2.PCur().ToStdVector());
+
+                    std::vector<Type> 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<tREAL8> 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