Merge branch 'mpd'

Author: deseilligny

MMVII/MMVII-UseCaseDataSet/Tests/Cmd-NewBlock.sh

@@ -2,20 +2,37 @@
 
 set -e
 
+MMVII BlockInstrEdit BL0 FromScratch=1
 MMVII BlockInstrEdit BL0 "PatsIm4Cam=[Im-Calib.xml,.*_(.*).tif]"  #   InMeasureClino=MesClinFilA_043
 MMVII BlockInstrEdit BL0 InMeasureClino=MesClinFilA_043
 MMVII BlockInstrEdit BL0  CRO=[[A1,A2,1e-6,i-kj]]
 MMVII BlockInstrEdit BL0  CRO=[[B1,B2,1e-6,i-kj]]
 
 
+# ----------------  0 Block inside pannel ----------------------------
 
+#   --- 0.1 Initiale
 MMVII BlockInstrInitCam Im-Calib_OK_Resec.xml BL0 Pannel_BA2_311 BL1    
 
-
+#   --- 0.2  Adjust
 MMVII OriBundleAdj Im-Calib_OK_Resec.xml  Pannel_BA2_311  Pannel_BA3_311 "GCP3D=[[Pannel,1]]" "GCP2D=[[Pannel,0.1,0.2,1,2]]" InInstrBlock=BL1 OutInstrBlock=BL2   BOI=[[,0.01,0.01,1],[]] NbIter=10
 
 
-#MMVII BlockInstrInitClino Im-Calib_OK_Resec.xml BL2 Pannel_BA3_311 
+# ----------------  1 Block on polyg ----------------------------
+
+
+MMVII OriBundleAdj Im-Clino_OK_Resec.xml  BA_UC_Clino_311  BA2_Clino_RigPannel  GCP3D=[[Polyg,1]] GCP2D=[[Polyg-Completed,0.1,0.5,5,2]] PPFzCal=.* InInstrBlock=BL2 OutInstrBlock=BL3    BOI=[[,0.001,0.001,1],[],[0.001,0.001]] NbIter=40
+
+MMVII OriBundleAdj Im-Clino_OK_Resec.xml  BA_UC_Clino_311  BA2_Clino_RigPolyg  GCP3D=[[Polyg,1]] GCP2D=[[Polyg-Completed,0.1,0.5,5,2]] PPFzCal=.* InInstrBlock=BL2 OutInstrBlock=BL4    BOI=[[,0.001,0.001,1],[],] NbIter=40
+
+
+MMVII BlockInstrInitClino Im-Clino_OK_Resec.xml BL3 BA2_Clino_RigPannel  MesClinPolyg_043  BL5
+
+MMVII BlockInstrInitClino Im-Clino_OK_Resec.xml BL3 BA2_Clino_Rig  MesClinPolyg_043  BL5
+
+
+#BRDirIn=BlockAdjust_${ImM} BRW=[5e-5,2e-5] OutRigBlock=BlocClino_${ImM}
+
 
 
 #MMVII BlockInstrEdit BL2  InMeasureClino=MesClinFilA_043 OutInstrBlock=BL3

MMVII/include/MMVII_2Include_Serial_Tpl.h

@@ -365,18 +365,19 @@ template <class Type> void AddData(const cAuxAr2007 & anAux, cDenseVect<Type>& a
 }
 
 /// if we want a human readable vector for small vect ..
-template <class Type> void AddDataAsStdVect(const cAuxAr2007 & anAux,cDenseVect<Type>& aDenseV)
+template <class Type> void AddDataAsStdVect(const cAuxAr2007 & anAux0,cDenseVect<Type>& aDenseV)
 {
+     cAuxAr2007 anAuxVect("StdVect",anAux0);
      std::vector<Type> aStdV;
-     if (anAux.Input())
+     if (anAux0.Input())
      {
-        MMVII::AddData(anAux,aStdV);
+        MMVII::AddData(anAuxVect,aStdV);
         aDenseV = cDenseVect<Type>(aStdV);
      }
      else
      {
         aStdV = aDenseV.ToStdVect();
-        MMVII::AddData(anAux,aStdV);
+        MMVII::AddData(anAuxVect,aStdV);
      }
 }
 

MMVII/include/MMVII_InstrumentalBlock.h

@@ -142,6 +142,9 @@ class cIrbCal_CamSet  : public cMemCheck
          std::vector<cIrbCal_Cam1> &      VCams();
          cIrbCal_Cam1 &                   MasterCam();
 
+         void SetNumPoseInstr (const std::vector<int> & aVNums);
+         // Correct -1 => master
+         std::vector<int>  NumPoseInstr() const;
      private :
          void AddCam
               (
@@ -152,6 +155,8 @@ class cIrbCal_CamSet  : public cMemCheck
               );
 
          int                             mNumMaster;      //< num of "master" image
+         // special case [] -> all  [-1]  master
+         std::vector<int>                mNumsPoseInstr;
          std::vector<cIrbCal_Cam1>       mVCams;          //< set of camerascIrbCal_Block
          cIrbCal_Block *                 mCalBlock;
 };
@@ -168,11 +173,18 @@ class cIrbCal_Clino1   : public cMemCheck
         cIrbCal_Clino1(const std::string & aName); //< "Real" constructor
         const std::string & Name() const;  //< accessor
         void AddData(const  cAuxAr2007 & anAux); //< serializer
+
+        void SetPNorm(const cPt3dr & aTr);
+
+        cP3dNormWithUK&  CurPNorm();
+        bool          IsInit() const;
+        void UnInit();
     private :
-        std::string   mName;           //< name of the clino
-    bool          mIsInit;         //< was values computed ?
-        tRotR         mOrientInBloc;    //< Position in the block
-        tREAL8        mSigmaR;         //< sigma on orientation
+        std::string        mName;             //< name of the clino
+        bool               mIsInit;           //< was values computed ?
+        cP3dNormWithUK *   mTrInBlock;        //< Position in the block
+        std::vector<tREAL8> mPolCorr;         //<  Polynomial correction 2 angles, def [0,1,0]
+       // tREAL8             mSigmaR;         //< sigma a priori  on orientation
 };
 void AddData(const  cAuxAr2007 & anAux,cIrbCal_Clino1 & aClino);
 
@@ -186,9 +198,14 @@ class cIrbCal_ClinoSet  : public cMemCheck
          cIrbCal_ClinoSet();
          void AddData(const  cAuxAr2007 & anAux);
          std::vector<std::string> VNames() const;
+         size_t NbClino() const;
+
+         int  IndexClinoFromName(const std::string& aName) const;
+         cIrbCal_Clino1 &  KthClino(int aK);
+
      private :
          cIrbCal_Clino1 * ClinoFromName(const std::string& aName);
-         void AddClino(const std::string &,bool SVP=false);
+         void AddClino(const std::string &,tREAL8 aSigma,bool SVP=false);
 
          std::vector<cIrbCal_Clino1> mVClinos; //< set of clinos
          cIrbCal_Block *              mCalBlock;
@@ -212,6 +229,17 @@ class cIrb_CstrRelRot
 
 void AddData(const  cAuxAr2007 & anAux,cIrb_CstrRelRot & aSigma);
 
+class cIrb_CstrOrthog
+{
+   public :
+      cIrb_CstrOrthog(const tREAL8 & aSigma);
+      cIrb_CstrOrthog();
+      void AddData(const  cAuxAr2007 & anAux);
+   private :
+      tREAL8 mSigma;
+};
+void AddData(const  cAuxAr2007 & anAux,cIrb_CstrOrthog & aSigma);
+
 
 /*
 class cIrb_ConstrInstr
@@ -302,7 +330,11 @@ class cIrbCal_Block  : public cMemCheck
          void AvgIndivSigma();  //< Set all sigma of object ir global average
          void AvgSigma();
          cIrb_Desc1Intsr &  AddSigma_Indiv(std::string aN1,eTyInstr aType1);
+
          void AddCstrRelRot(std::string aN1,std::string aN2,tREAL8 aSigma,tRotR aRot);
+         void AddCstrRelOrthog(std::string aN1,std::string aN2,tREAL8 aSigma);
+
+         const std::map<tNamePair,cIrb_CstrOrthog> &  CstrOrthog() const;
 
      private :
 
@@ -318,9 +350,15 @@ class cIrbCal_Block  : public cMemCheck
          cIrbCal_CamSet                mSetCams;    //<  Cameras used in the bloc
          cIrbCal_ClinoSet              mSetClinos;  //<  Clinos used in the bloc
 
+         //  A priori external constraint
          std::map<tNamePair,cIrb_SigmaInstr>   mSigmaPair;     //<  Sigmas between pair of instr
          std::map<std::string,cIrb_Desc1Intsr> mDescrIndiv;      //<  Sigmas of each instrument
+
+         //  A priori external constraint
          std::map<tNamePair,cIrb_CstrRelRot>   mCstrRelRot;
+         std::map<tNamePair,cIrb_CstrOrthog>   mCstrOrthog;
+
+
 
 };
 void AddData(const  cAuxAr2007 & anAux,cIrbCal_Block & aRBoI);
@@ -382,6 +420,7 @@ class cIrbComp_Clino1 : public cMemCheck
 {
    public :
         cIrbComp_Clino1(tREAL8 anAngle);
+        tREAL8 Angle() const;
    private :
         tREAL8 mAngle;
 };
@@ -391,6 +430,8 @@ class cIrbComp_ClinoSet : public cMemCheck
    public :
       cIrbComp_ClinoSet();
       void SetClinoValues(const cOneMesureClino&);
+      const cIrbComp_Clino1 & KthMeasure(int aK) const;
+      size_t NbMeasure() const;
 
    private :
        std::vector<cIrbComp_Clino1>  mVCompClinos;
@@ -405,15 +446,19 @@ class   cIrbComp_TimeS : public cMemCheck
          cIrbComp_TimeS (const cIrbComp_Block &);
          const cIrbComp_CamSet & SetCams() const;  //< Accessor
           cIrbComp_CamSet & SetCams();  //< Accessor
+          const cIrbComp_ClinoSet & SetClino() const;
 
          const cIrbComp_Block & CompBlock() const; //< Accessor
          const cIrbCal_Block & CalBlock() const; //< Accessor or Accessor
          // cIrbComp_Block & CompBlock() ; //< Accessor
 
          // if not SVP and cannot compute : error
-         void ComputePoseInstrument(bool SVP = false);
+         void ComputePoseInstrument(const std::vector<int> & aVNumCam,bool SVP = false);
          void SetClinoValues(const cOneMesureClino&);
 
+         tREAL8 ScoreDirClino(const cPt3dr& aDir,size_t aKClino) const;
+
+
     private :
          cIrbComp_TimeS(const cIrbComp_TimeS&) = delete;
          const cIrbComp_Block &            mCompBlock;
@@ -468,6 +513,9 @@ class   cIrbComp_Block : public cMemCheck
        void SetClinoValues(const cSetMeasureClino&,bool OkNewTimeS=false );
        /// call previous by using std measure on phproj
        void SetClinoValues(bool OkNewTimeS=false);
+
+       tREAL8 ScoreDirClino(const cPt3dr& aDir,size_t aKClino) const;
+
     private :
        /// non copiable, too "dangerous"
        cIrbComp_Block(const cIrbComp_Block & ) = delete;

MMVII/include/MMVII_PCSens.h

@@ -450,7 +450,7 @@ class cPoseWithUK :  public cObjWithUnkowns<tREAL8>
         */
 	 void PushObs(std::vector<double> &,bool TransposeMatr);
 
-	 cPoseWithUK(const tPoseR & aPose);
+         cPoseWithUK(const tPoseR & aPose);
          void SetPose(const tPoseR & aPose);
 
 	 // different accessor to the pose
@@ -467,24 +467,47 @@ class cPoseWithUK :  public cObjWithUnkowns<tREAL8>
          tPoseR &   Pose()   ;
 
 	 /// The interval is specified as the object can be used as helperto to other classes
-         void PutUknowsInSetInterval(cSetInterUK_MultipeObj<tREAL8> * aSetInterv) ;  
+         void PutUknowsInSpecifiedSetInterval(cSetInterUK_MultipeObj<tREAL8> * aSetInterv) ;
          void OnUpdate() override;                 // "reaction" after linear update
-	 void  FillGetAdrInfoParam(cGetAdrInfoParam<tREAL8> &) override;
+         void  FillGetAdrInfoParam(cGetAdrInfoParam<tREAL8> &) override;
 	 // std::vector<tPtrOUK>  GetAllUK() override;
 
          // Val axiator should have to "equal" fix rot
          cPt3dr ValAxiatorFixRot(const cRotation3D<tREAL8> & aRotFix) const;
 
 
      private :
-	 cPoseWithUK(const cPoseWithUK&) = delete;
+         cPoseWithUK(const cPoseWithUK&) = delete;
          void PutUknowsInSetInterval() override ;  // add the interval on udpate
 
          tPoseR     mPose;   ///< transformation Cam to Word
          cPt3dr     mOmega;  ///< vector for tiny rotation when used in unknown, mW  in code gene ...
 };
 void AddData(const  cAuxAr2007 & anAux,cPoseWithUK & aPUK);
 
+///  class for modelizing a normaliezd 3D-Vect when used as unknown in non linear system
+class cP3dNormWithUK :  public cObjWithUnkowns<tREAL8>
+{
+   public :
+        cP3dNormWithUK(const cPt3dr &aPt,const std::string& aNameType,const std::string & aNameGrp);
+        ~cP3dNormWithUK();
+        void OnUpdate() override;                 // "reaction" after linear update
+        void  FillGetAdrInfoParam(cGetAdrInfoParam<tREAL8> &) override;
+
+        void SetPNorm(const cPt3dr & aTr);
+        cPt3dr GetPNorm () const;
+   private :
+        // normalize , compute U & V, set mDuDv to 0
+      void Init();
+      void PutUknowsInSetInterval() override ;
+
+      cPt3dr  mPNorm;  // curent value of PNorm
+      cPt3dr  mU;      // first vector Orthog to mPNorm
+      cPt3dr  mV;      // seconf vector Orthog to PNorm
+      cPt2dr  mDuDv;   //
+
+};
+
 
 /**  Class for modelizing the geometry of perspective-central image, contain essentially a pose (Centre+rotation)
  *   and a pointer to a (generally) shared internall calibration

MMVII/include/MMVII_Ptxd.h

@@ -569,6 +569,8 @@ template <class Type> inline bool InfEq  (const cPtxd<Type,4> & aP1,const cPtxd<
 
 
 template<class T,const int Dim> cPtxd<T,Dim>  VUnit(const cPtxd<T,Dim> & aP);
+template <class TypePt> std::pair<TypePt,TypePt> OrthogonalizePair(const TypePt & aP1,const TypePt & aP2);
+
 // template<const int Dim> cPtxd<tREAL8 ,Dim>  VUnit(const cPtxd<tREAL8 ,Dim> & aP);
 // template<const int Dim> cPtxd<tREAL16,Dim>  VUnit(const cPtxd<tREAL16,Dim> & aP);
 

MMVII/include/MMVII_Stringifier.h

@@ -133,6 +133,8 @@ template <>  const std::string cStrIO<eTopoObsType>::msNameType;
 template <>  const std::string cStrIO<eTopoObsSetType>::msNameType;
 template <>  const std::string cStrIO<eTopoStOriStat>::msNameType;
 template <>  const std::string cStrIO<eTyClino>::msNameType;
+template <>  const std::string cStrIO<eTyInstr>::msNameType;
+
 #endif
 
 /** These functions offer an"easy" interface to cStrIO, however I think

MMVII/include/MMVII_enums.h

@@ -279,6 +279,7 @@ enum class eTyUnitAngle
               eUA_radian,
               eUA_degree,
               eUA_gon,
+              eUA_DMgon,
               eNbVals
 	   };
 

MMVII/include/MMVII_nums.h

@@ -16,7 +16,11 @@ template <class Type> Type Average(const Type * aTab,size_t aNb);
 template <class Type> Type Average(const std::vector<Type> &);
 
 
-tREAL8 AngleInRad(eTyUnitAngle);
+tREAL8 AngleFromRad(eTyUnitAngle);
+tREAL8 AngleFromRad(tREAL8 aAngInRad,eTyUnitAngle);
+tREAL8 Rad2DMgon(tREAL8 aAngInRad);
+
+
 bool AssertRadAngleInOneRound(tREAL8 aAngleRad, bool makeError=true);
 
 // some time needs a null val for any type with + (neutral for +)

MMVII/include/MMVII_util_tpl.h

@@ -6,6 +6,8 @@
 #include "MMVII_enums.h"
 #include "MMVII_Error.h"
 #include "MMVII_nums.h"
+#include "MMVII_Stringifier.h"
+
 
 namespace MMVII
 {
@@ -268,7 +270,7 @@ template<class TCont,class TKey> const typename  TCont::mapped_type *  MapGet(co
     auto anIter = aCont.find(aKey);
     if (anIter==aCont.end())
     {
-        MMVII_INTERNAL_ASSERT_tiny(SVP,"MapGet");
+        MMVII_INTERNAL_ASSERT_tiny(SVP,"MapGet for Key="+ cStrIO<TKey>::ToStr(aKey));
         return nullptr;
     }
     return & anIter->second;
@@ -591,6 +593,22 @@ class cComputeAssociation
 };
 void AddData(const cAuxAr2007 & anAux,cComputeAssociation & aTransl);
 
+
+
+template <class TypeCont> typename TypeCont::value_type SumElem(const TypeCont &aCont)
+{
+    typename TypeCont::value_type  aResult = cNV<typename TypeCont::value_type>::V0();
+    for (const auto aVal : aCont)
+        aResult += aVal;
+    return aResult;
+}
+
+template <class TypeCont> typename TypeCont::value_type AvgElem(const TypeCont &aCont)
+{
+    MMVII_INTERNAL_ASSERT_tiny(!aCont.empty(),"AvgElem on empty vect");
+    return SumElem(aCont) / double (aCont.size());
+}
+
 };
 
 #endif  //  _MMVII_Util_TPL_H_