Some topo

MMVII/Doc/Programmer/NonLinearOptim.tex

@@ -1202,16 +1202,22 @@ \subsection{Global presentation}
 
 All points must be given approximative coordinates as there is no automatic initialization.
 
-For now it is only used in the Bench \texttt{TopoComp}.
+For now it is used in \texttt{OriBundleAdj} and in the Bench \texttt{TopoComp}.
+
 This Bench will be used to illustrate the topometric classes and their usage.
 
 
 \subsection{\texttt{TopoComp} Bench example}
 \label{subsec:topoBench}
 
-The example is created by the method \texttt{cTopoComp::createEx1()}.
+The example is created by the method \texttt{cTopoData::createEx1()}.
+
+The classes named \texttt{cTopo???Data} are used for data serialization and deserialization.
+They are a simplified version of the classes without the \texttt{Data} suffix, that are used in
+the actual computation.
+
 
-\subsubsection{Step 1}
+\subsubsection{Creating the fixed points}
 
 At first, there are 3 fixed points ($A$, $B$, $C$), forming an isosceles triangle
 on an horizontal plane (Fig. \ref{fig:topoEx1}).
@@ -1223,28 +1229,24 @@ \subsubsection{Step 1}
 \label{fig:topoEx1}
 \end{figure}
 
-The points are instances of the \texttt{cTopoPoint} class that
-derives from \texttt{cObjWithUnkowns}, hence it has no copy constructor and
-the points have to be added as pointers to \texttt{cTopoComp::allPts()}.
+The points are instances of the \texttt{cTopoPoint} class.
+The false boolean in the constructor means that the points are not free.
 
 \begin{lstlisting}
-  //create fixed points
-  allPts.push_back(new cTopoPoint("ptA", cPt3dr(10,10,10), false));
-  allPts.push_back(new cTopoPoint("ptB", cPt3dr(20,10,10), false));
-  allPts.push_back(new cTopoPoint("ptC", cPt3dr(15,20,10), false));
-  auto ptA = allPts[0];
-  auto ptB = allPts[1];
-  auto ptC = allPts[2];
+    //create fixed points
+    cTopoPointData ptA = {"ptA",cPt3dr(10,10,10),false};
+    cTopoPointData ptB = {"ptB",cPt3dr(20,10,10),false};
+    cTopoPointData ptC = {"ptC",cPt3dr(15,20,10),false};
 \end{lstlisting}
 
 At this point, there are no unknowns and no observations.
 
 
-\subsubsection{Step 2}
+\subsubsection{Creating the observations}
 
 A fourth point ($D$), this time not fixed, is initialized above the $ABC$ triangle.
-Distances from $A$, $B$ and $C$ to $D$
-are measured. For redondancy and error evaluation, the distance from $C$ to $D$ is measured twice
+Distances from $D$ to $A$, $B$ and $C$
+are measured. For redondancy and error evaluation, the distance from $D$ to $C$ is measured twice
 with different values (Fig. \ref{fig:topoEx2}).
 
 \begin{figure}[!h]
@@ -1254,29 +1256,43 @@ \subsubsection{Step 2}
 \label{fig:topoEx2}
 \end{figure}
 
-Observations must refer to an observation set (deriving from class \texttt{cTopoObsSet})
-that is used to share parameters between observations.
+Observations are represented with the \texttt{cTopoObs} class.
+The constructor needs a \texttt{eTopoObsType}, the list of affected points
+(in order : "from", "to"), and a list of measurements and their sigmas.
 
-In the case of measured distances there is no parameter, therefore the observation set will be 
-a \texttt{cTopoObsSetSimple}. Observations sets must be created with \texttt{make\_TopoObsSet} template function.
+\begin{lstlisting}
+    cTopoPointData ptD = {"ptD",cPt3dr(14,14,14),true};
+  #define WW 0.01
+    //add measured dist to point D
+    cTopoObsData aObs1 = {eTopoObsType::eDist, {"ptD", "ptA"},  {10.}, {WW}};
+    cTopoObsData aObs2 = {eTopoObsType::eDist, {"ptD", "ptB"},  {10.}, {WW}};
+    cTopoObsData aObs3 = {eTopoObsType::eDist, {"ptD", "ptC"},  {10.}, {WW}};
+    cTopoObsData aObs4 = {eTopoObsType::eDist, {"ptD", "ptC"},  {10+WW}, {WW}};
+\end{lstlisting}
 
-All topometric observations are instances of \texttt{cTopoObs} class.
-For distances observations, \texttt{TopoObsType::dist} is given to the constructor.
+The observations must be recorded in a \texttt{cTopoObsSet}, which is used to
+record parameters common to several observations.
+Here we must use the set type \texttt{Station} because we have distances (more about that later).
 
 \begin{lstlisting}
-  //add measured dist to point D
-  allObsSets.push_back(make_TopoObsSet<cTopoObsSetSimple>());
-  auto obsSet1 = allObsSets[0].get();
-  allPts.push_back(new cTopoPoint("ptD", cPt3dr(14,14,14), true));
-  auto ptD = allPts[3];
-  cTopoObs(obsSet1, TopoObsType::dist, std::vector{ptA, ptD}, {10.0});
-  cTopoObs(obsSet1, TopoObsType::dist, std::vector{ptB, ptD}, {10.0});
-  cTopoObs(obsSet1, TopoObsType::dist, std::vector{ptC, ptD}, {10.0});
-  cTopoObs(obsSet1, TopoObsType::dist, std::vector{ptC, ptD}, {11.0});
+    cTopoObsSetData aSet1;
+    aSet1.mType = eTopoObsSetType::eStation;
+    aSet1.mObs = {aObs1, aObs2, aObs3, aObs4};
 \end{lstlisting}
 
 The system now contains 3 unknowns ($D$ coordinates) and 4 observations
-(measured distances $AD$, $BD$ and $CD$ two times).
+(measured distances $DA$, $DB$ and $DC$ two times).
+
+All these objects have to be added to a \texttt{aTopoData} object:
+
+\begin{lstlisting}
+    cTopoData aTopoData;
+    aTopoData.mAllPoints = {ptA, ptB, ptC, ptD};
+    aTopoData.mAllObsSets = {aSet1, aSet2, aSet3};
+\end{lstlisting}
+
+\begin{comment}
+
 
 \subsubsection{Step 3}
 
@@ -1311,6 +1327,18 @@ \subsubsection{Step 3}
 This adds 4 unknowns ($E$ coordinates and the unknown distance $d$) and 4 observations
 ($EA = EB = EC = ED = d$).
 
+\end{comment}
+
+\subsubsection{Transform into computation objects}
+
+All these \texttt{Data} objects will be used to fill the actual computation
+objects.
+
+These clases are :
+
+\begin{lstlisting}
+
+\end{lstlisting}
 
 \subsection{Topometric computation}
 

MMVII/include/MMVII_PhgrDist.h

@@ -182,6 +182,11 @@ NS_SymbolicDerivative::cCalculator<double> * EqTopoSubFrame(bool WithDerive,int
 /// Sum of square of unknown, to test non linear constraints
 NS_SymbolicDerivative::cCalculator<double> * EqSumSquare(int aNb,bool WithDerive,int aSzBuf,bool ReUse);
 
+// .............   Equation for topo stations .............
+/// topo obs from a station: , Uk={pose_origin, pt_to} Obs={r00, r01, r02, r10, r11, r12, r20, r21, r22, val},
+NS_SymbolicDerivative::cCalculator<double> * EqTopoHz(bool WithDerive,int aSzBuf);
+NS_SymbolicDerivative::cCalculator<double> * EqTopoZen(bool WithDerive,int aSzBuf);
+NS_SymbolicDerivative::cCalculator<double> * EqTopoDist(bool WithDerive,int aSzBuf);
 
 
            // .............   Equation implying 2D distance conservation .............

MMVII/include/MMVII_Stringifier.h

@@ -442,7 +442,8 @@ void AddData(const  cAuxAr2007 & anAux, tU_INT1  &  aVal); ///< for unsigned sho
 void AddData(const  cAuxAr2007 & anAux, tINT2  &  aVal); ///< for unsigned short
 void AddData(const  cAuxAr2007 & anAux, tU_INT2  &  aVal); ///< for unsigned short
 void AddData(const  cAuxAr2007 & anAux, tREAL4  &  aVal); ///< for unsigned short
-
+void AddData(const  cAuxAr2007 & anAux, tREAL16  &  aVal); ///< for long double
+
 void AddData(const  cAuxAr2007 & anAux, size_t  &  aVal); ///< for unsigned short
 void AddData(const  cAuxAr2007 & anAux, double  &  aVal) ; ///< for double
 void AddData(const  cAuxAr2007 & anAux, std::string  &  aVal) ; ///< for string

MMVII/include/MMVII_SysSurR.h

@@ -2,6 +2,7 @@
 #define  _MMVII_SysSurR_H_
 
 #include "SymbDer/SymbDer_Common.h"
+#include "MMVII_2Include_Serial_Tpl.h"
 #include "MMVII_Matrix.h"
 
 namespace MMVII
@@ -51,11 +52,27 @@ template <class Type> class cResidualWeighterExplicit: public cResidualWeighter<
             tStdVect & getSigmas() { return mSigmas; }
             tStdVect & geWeights() { return mWeights; }
             int size() const { return mWeights.size(); }
+            void AddData(const  cAuxAr2007 & anAuxInit);
        private :
             tStdVect mSigmas;
             tStdVect mWeights;
 };
 
+template <class Type>
+void cResidualWeighterExplicit<Type>::AddData(const  cAuxAr2007 & anAuxInit)
+{
+    cAuxAr2007 anAux("ResidualWeighterExplicit",anAuxInit);
+
+    MMVII::AddData(cAuxAr2007("Sigmas",anAux),mSigmas);
+    MMVII::AddData(cAuxAr2007("Weights",anAux),mWeights);
+}
+
+template <class Type>
+void AddData(const cAuxAr2007 & anAux, cResidualWeighterExplicit<Type> &aWeighter)
+{
+    aWeighter.AddData(anAux);
+}
+
 
 template <class Type> class cREAL8_RWAdapt : public cResidualWeighter<Type>
 {
@@ -279,6 +296,7 @@ template <class Type> class cResolSysNonLinear : public cREAL8_RSNL
 
           void  AddConstr(const tSVect & aVect,const Type & aCste,bool OnlyIfFirstIter=true);
           void SupressAllConstr();
+          int GetNbLinearConstraints() const;
      private :
           cResolSysNonLinear(const tRSNL & ) = delete;
 
@@ -338,6 +356,9 @@ template <class Type> class cInputOutputRSNL
 
           // use a s converter from tREAL8, "Fake" is used to separate from copy construtcor when Type == tREAL8
 	  cInputOutputRSNL(bool Fake,const cInputOutputRSNL<tREAL8> &);
+
+      /// 4 Debug purpose
+      void Show() const;
      private :
 	  // cInputOutputRSNL(const cInputOutputRSNL<Type> &) = delete;
 
@@ -871,6 +892,7 @@ template <const int Dim>  class cPtxdr_UK :  public cObjWithUnkowns<tREAL8>,
       ~cPtxdr_UK();
       void PutUknowsInSetInterval() override;
       const tPt & Pt() const ;
+      tPt & Pt() ;
    private :
       cPtxdr_UK(const cPtxdr_UK&) = delete;
       tPt mPt;

MMVII/include/MMVII_enums.h

@@ -120,7 +120,6 @@ enum class eTAAr
 };
 
 
-
 /// Type of external format that are potentially imported/exported in MicMac
 enum class eFormatExtern
            {
@@ -537,6 +536,24 @@ enum class eSysCoGeo
      eNbVals
 };
 
+// topometric observation sets types
+enum class eTopoObsSetType
+{
+    eStation,
+    //eDistParam,
+    //eSubFrame,
+    eNbVals        ///< Tag for number of value
+};
+
+// topometric observations types
+enum class eTopoObsType
+{
+        eDist,
+        eHz,
+        eZen,
+        eNbVals        ///< Tag for number of value
+};
+
 
 enum class eTyCodeTarget
 {
@@ -567,9 +584,11 @@ const std::string & E2Str(const eMTDIm &);
 const std::string & E2Str(const eFormatExtern &);
 const std::string & E2Str(const eTypeSerial &);
 const std::string & E2Str(const eTAAr &);
-const std::string & E2Str(const eProjPC &);         
+const std::string & E2Str(const eProjPC &);
 const std::string & E2Str(const eSysCoGeo &);         
-const std::string & E2Str(const eDCTFilters &);         
+const std::string & E2Str(const eTopoObsSetType &);
+const std::string & E2Str(const eTopoObsType &);
+const std::string & E2Str(const eDCTFilters &);
 const std::string & E2Str(const eTyCodeTarget &);         
 const std::string & E2Str(const eTySC &);         
 const std::string & E2Str(const eOpAff &);         

MMVII/src/BundleAdjustment/BundleAdjustment.h

@@ -1,12 +1,18 @@
+#ifndef  _MMVII_BUNDLEADJUSTMENT_H_
+#define  _MMVII_BUNDLEADJUSTMENT_H_
+
 #include "MMVII_PCSens.h"
 #include "MMVII_MMV1Compat.h"
 #include "MMVII_DeclareCste.h"
 #include "MMVII_BundleAdj.h"
 
+
 using namespace NS_SymbolicDerivative;
 namespace MMVII
 {
 
+class cBA_Topo;
+
 /**   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
@@ -119,7 +125,7 @@ class cMMVII_BundleAdj
 
 	  void AddBlocRig(const std::vector<double>& aSigma,const std::vector<double>&  aSigmRat ); // RIGIDBLOC
 	  void AddCamBlocRig(const std::string & aCam); // RIGIDBLOC
-
+          bool AddTopo(const std::string & aTopoFilePath); // TOPO
           ///  =======  Add GCP, can be measure or measure & object
           void AddGCP(tREAL8 aSigmaGCP,const  cStdWeighterResidual& aWeightIm, cSetMesImGCP *);
 
@@ -149,6 +155,7 @@ class cMMVII_BundleAdj
 
 	  void SaveBlocRigid();
           void Save_newGCP();
+          void SaveTopo();
 
      private :
 
@@ -161,6 +168,7 @@ class cMMVII_BundleAdj
           void AssertPhpAndPhaseAdd() ;  /// Assert both
           void InitIteration();          /// Called at first iteration -> Init things and set we are non longer in Phase Add
           void InitItereGCP();           /// GCP Init => create UK
+          void InitItereTopo();           /// Topo Init => create UK
           void OneItere_GCP();           /// One iteraion of adding GCP measures
 
 	  void OneItere_TieP();   /// Iteration on tie points
@@ -210,7 +218,8 @@ class cMMVII_BundleAdj
 
                  // - - - - - - -   Bloc Rigid - - - - - - - -
 	  cBA_BlocRig*              mBlRig;  // RIGIDBLOC
-
+          cBA_Topo*              mTopo;  // TOPO
+
 	         // - - - - - - -   Reference poses- - - - - - - -
           std::vector<cSensorCamPC *>        mVCamRefPoses;      ///< vector of reference  poses if they exist
 	  std::string                        mFolderRefCam;
@@ -228,3 +237,4 @@ class cMMVII_BundleAdj
 
 };
 
+#endif // _MMVII_BUNDLEADJUSTMENT_H_

MMVII/src/BundleAdjustment/Bundle_GCP.cpp

@@ -184,13 +184,13 @@ template <const int Dim> void cPtxdr_UK<Dim>::PutUknowsInSetInterval()
 {
     mSetInterv->AddOneInterv(mPt);
 }
-template <const int Dim>  const cPtxd<tREAL8,Dim> & cPtxdr_UK<Dim>::Pt() const {return mPt;}
 
+template <const int Dim>  const cPtxd<tREAL8,Dim> & cPtxdr_UK<Dim>::Pt() const {return mPt;}
+template <const int Dim>  cPtxd<tREAL8,Dim> & cPtxdr_UK<Dim>::Pt() {return mPt;}
 
 template class cPtxdr_UK<2>;
 template class cPtxdr_UK<3>;
 
-
 };
 
 

MMVII/src/BundleAdjustment/cAppliBundAdj.cpp

@@ -4,6 +4,7 @@
    \file cAppliBundAdj.cpp
 
 */
+#include "../Topo/Topo.h"
 
 /*
     Track info on bundle adj/push broom in V1 :
@@ -77,6 +78,9 @@ class cAppliBundlAdj : public cMMVII_Appli
         std::vector<std::string>  mParamRefOri;
 
 	int                       mNbIter;
+
+        std::string               mTopoFilePath;  // TOPO
+
 	std::string               mPatParamFrozCalib;
 	std::string               mPatFrosenCenters;
 	std::string               mPatFrosenOrient;
@@ -136,8 +140,11 @@ cCollecSpecArg2007 & cAppliBundlAdj::ArgOpt(cCollecSpecArg2007 & anArgOpt)
       << AOpt2007(mLVM,"LVM","Levenberg–Marquardt parameter (to have better conditionning of least squares)",{eTA2007::HDV})
       << AOpt2007(mBRSigma,"BRW","Bloc Rigid Weighting [SigmaCenter,SigmaRot]",{{eTA2007::ISizeV,"[2,2]"}})  // RIGIDBLOC
       << AOpt2007(mBRSigma_Rat,"BRW_Rat","Rattachment fo Bloc Rigid Weighting [SigmaCenter,SigmaRot]",{{eTA2007::ISizeV,"[2,2]"}})  // RIGIDBLOC
+
       << 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(mTopoFilePath,"TopoFile","Topo obs file path")    //TOPO
     ;
 }
 
@@ -220,6 +227,12 @@ int cAppliBundlAdj::Exe()
             mBA.AddCamBlocRig(aNameIm);
     }
 
+    if (IsInit(&mTopoFilePath))
+    {
+        bool aTopoOk = mBA.AddTopo(mTopoFilePath);
+        MMVII_INTERNAL_ASSERT_tiny(aTopoOk,"Error reading topo obs file "+mTopoFilePath);
+    }
+
     MMVII_INTERNAL_ASSERT_User(MeasureAdded,eTyUEr::eUnClassedError,"Not any measure added");
 
     for (int aKIter=0 ; aKIter<mNbIter ; aKIter++)
@@ -238,6 +251,7 @@ int cAppliBundlAdj::Exe()
 
     mBA.SaveBlocRigid();  // RIGIDBLOC
     mBA.Save_newGCP();
+    mBA.SaveTopo(); // just for debug for now
 
     return EXIT_SUCCESS;
 }