Merge branch 'mpd'

MMVII/Doc/Methods/LidarImageRegistration.tex

@@ -394,32 +394,32 @@ \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}
 \end{equation}
 
 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,15 +434,15 @@ \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$
    \item $2 + N*M$ equations;		 
 \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}.
 

MMVII/include/MMVII_2Include_Tiling.h

@@ -289,6 +289,31 @@ template <const int Dim> class cPointSpInd
 };
 
 
+/**  Class for geometrically indexing the lidars (on 2D point) for patches creation , used
+   to instantiate cTilingIndex 
+*/
+
+template <class Type> class cTil2DTri3D
+{
+    public :
+        static constexpr int TheDim = 2;          // Pre-requite for instantite cTilingIndex
+        typedef cPt2dr             tPrimGeom;     // Pre-requite for instantite cTilingIndex
+        typedef const cTriangulation3D<Type> *  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 <const int TheDim> class cGeneratePointDiff
 {
@@ -373,5 +398,6 @@ template <class TypePrim,class TypeObj,class TypeCalcP2 >
 
 
 
+
 };
 #endif //   _MMVII_Tiling_H_

MMVII/include/MMVII_Geom3D.h

@@ -329,12 +329,19 @@ template <class Type> class cTriangulation3D : public cTriangulation<Type,3>
 
 	   cTriangle<Type,2>     TriDevlpt(int aKF,int aNumSom) const;  // aNumSom in [0,1,2]
 	   cDevBiFaceMesh<Type>  DoDevBiFace(int aKF1,int aNumSom) const;  // aNumSom in [0,1,2]
+
+           cBox2dr  Box2D() const;
+
+           void MakePatches(std::list<std::vector<int> > & ,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 :

MMVII/include/MMVII_SysSurR.h

@@ -231,6 +231,7 @@ template <class Type> 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 Type> 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 Type> class cLinearOverCstrSys  : public cMemCheck
 
 
       virtual void   AddCov(const cDenseMatrix<Type> &,const cDenseVect<Type>& ,const std::vector<int> &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<Type> & aCoeff,const Type &  aRHS) ;
+
+       void AddWRHS(Type aW,Type aRHS);
+
+    private :
+
+       cDenseVect<Type>  mLVMW;             ///< The Levenberg markad weigthing
+       // mSumWCoeffRHS are update at PublicAddObs, reseted at PublicReset, used at PublicSolve
+       cDenseVect<Type>  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<Type> & aCoeff,const Type &  aRHS) = 0;
        /// Add  aPds (  aCoeff .X = aRHS) , version sparse
        virtual void SpecificAddObservation(const Type& aWeight,const cSparseVect<Type> & aCoeff,const Type &  aRHS) = 0;
 
-       Type LVMW(int aK) const;
 
-    protected :
-       int mNbVar;
-       cDenseVect<Type>  mLVMW;  // The Levenberg markad weigthing
-    private :
        /// "Purge" all accumulated equations
        virtual void SpecificReset() = 0;
        /// Compute a solution

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 <const int Dim>  class cPtxdr_UK :  public cObjWithUnkowns<tREAL8>,
-                                             public cMemCheck
-{
-   public :
-      typedef cPtxd<tREAL8,Dim>  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<std::pair<tREAL8,cPt2dr>> 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<std::pair<tREAL8,cPt2dr>> 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<std::string> & 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<cPt3dr> & aPatch);
+
+       /// Method for adding observations with radiometric differences as similatity criterion
+       void AddPatchDifRad(tREAL8 aW,const std::vector<cPt3dr> & aPatch,const std::vector<cData1ImLidPhgr> &aVData) ;
+
+       /// Method for adding observations with Census Coeff as similatity criterion
+       void AddPatchCensus(tREAL8 aW,const std::vector<cPt3dr> & aPatch,const std::vector<cData1ImLidPhgr> &aVData) ;
+
+       /// Method for adding observations with Normalized Centred Coefficent Correlation as similatity criterion
+       void AddPatchCorrel(tREAL8 aW,const std::vector<cPt3dr> & aPatch,const std::vector<cData1ImLidPhgr> &aVData) ;
+
        void SetVUkVObs
        (
             const cPt3dr&           aPGround,
@@ -380,17 +372,17 @@ class cBA_LidarPhotogra
        );
 
 
-       cMMVII_BundleAdj&               mBA;
-       eImatchCrit                     mNumMode;
-       cTriangulation3D<tREAL4>        mTri;
-       cDiffInterpolator1D *           mInterp;
-       cCalculator<double>  *          mEqLidPhgr;
-       std::vector<cSensorCamPC *>     mVCam;
-       std::vector<cIm2D<tU_INT1>>     mVIms;
-       cWeightAv<tREAL8,tREAL8>        mLastResidual;
-       std::list<std::vector<int> >    mLPatches;
-       bool                            mPertRad;
-       size_t                          mNbPointByPatch;
+       cMMVII_BundleAdj&              mBA;             ///< The global bundle adj structure
+       eImatchCrit                    mModeSim;        ///< type of similarity used
+       cTriangulation3D<tREAL4>       mTri;            ///< Triangulation, in fact used only for points 
+       cDiffInterpolator1D *          mInterp;         ///< Interpolator, used to extract  Value & Grad of images
+       cCalculator<double>  *         mEqLidPhgr;      ///< Calculator used for constrain the pose from image obs
+       std::vector<cSensorCamPC *>    mVCam;           ///< Vector of central perspective camera
+       std::vector<cIm2D<tU_INT1>>    mVIms;           ///< Vector of images associated to each cam
+       cWeightAv<tREAL8,tREAL8>       mLastResidual;   ///< Accumulate the radiometric residual
+       std::list<std::vector<int> >   mLPatches;       ///< set of 3D patches
+       bool                           mPertRad;        ///< do we pertubate the radiometry (simulation & test)
+       size_t                         mNbPointByPatch; ///< (approximate) required number of point /patch
 };