topo: add obs dx dy dz

micmacIGN · Feb 22, 2024 · f0d1f02 · f0d1f02
1 parent a9b4f85
commit f0d1f02
Show file tree

Hide file tree

Showing 10 changed files with 173 additions and 3 deletions.
diff --git a/MMVII/include/MMVII_PhgrDist.h b/MMVII/include/MMVII_PhgrDist.h
@@ -187,6 +187,9 @@ NS_SymbolicDerivative::cCalculator<double> * EqSumSquare(int aNb,bool WithDerive
 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);
+NS_SymbolicDerivative::cCalculator<double> * EqTopoDX(bool WithDerive,int aSzBuf);
+NS_SymbolicDerivative::cCalculator<double> * EqTopoDY(bool WithDerive,int aSzBuf);
+NS_SymbolicDerivative::cCalculator<double> * EqTopoDZ(bool WithDerive,int aSzBuf);
 
 
            // .............   Equation implying 2D distance conservation .............

diff --git a/MMVII/include/MMVII_enums.h b/MMVII/include/MMVII_enums.h
@@ -551,6 +551,9 @@ enum class eTopoObsType
         eDist,
         eHz,
         eZen,
+        eDX,
+        eDY,
+        eDZ,
         eNbVals        ///< Tag for number of value
 };
 

diff --git a/MMVII/src/Serial/ElemStrToVal.cpp b/MMVII/src/Serial/ElemStrToVal.cpp
@@ -183,6 +183,9 @@ template<> cE2Str<eTopoObsType>::tMapE2Str cE2Str<eTopoObsType>::mE2S
                 {eTopoObsType::eHz,"Hz"},
                 {eTopoObsType::eZen,"Zen"},
                 {eTopoObsType::eDist,"Dist"},
+                {eTopoObsType::eDX,"DX"},
+                {eTopoObsType::eDY,"DY"},
+                {eTopoObsType::eDZ,"DZ"},
                 //{eTopoObsType::eSubFrame,"SubFrame"},
                 //{eTopoObsType::eDistParam,"DistParam"},
            };

diff --git a/MMVII/src/SymbDerGen/Formulas_Topo.h b/MMVII/src/SymbDerGen/Formulas_Topo.h
@@ -143,6 +143,118 @@ class cFormulaTopoDist
 };
 
 
+
+class cFormulaTopoDX
+{
+      public :
+
+           std::string FormulaName() const { return "TopoDX";}
+
+           std::vector<std::string>  VNamesUnknowns()  const
+           {
+                //  Instrument pose with 6 unknowns : 3 for centers, 3 for axiator
+               // target pose with 3 unknowns : 3 for center
+                return  Append(NamesPose("Ci","Wi"),NamesP3("P_to"));
+           }
+
+           std::vector<std::string>    VNamesObs() const
+           {
+                // for the instrument pose, the 3x3 current rotation matrix as "observation/context"
+                // and the measure value
+                return  Append(NamesMatr("mi",cPt2di(3,3)), {"val"} );
+           }
+
+           template <typename tUk>
+                       std::vector<tUk> formula
+                       (
+                          const std::vector<tUk> & aVUk,
+                          const std::vector<tUk> & aVObs
+                       ) const
+           {
+               cPoseF<tUk>  aPoseInstr(aVUk,0,aVObs,0,true);
+               cPtxd<tUk,3> aP_to = VtoP3(aVUk,6);
+               auto       val = aVObs[9];
+               cPtxd<tUk,3>  aP_to_instr = aPoseInstr.Inverse().Value(aP_to);
+
+               return {  aP_to_instr.x() - val };
+           }
+};
+
+
+class cFormulaTopoDY
+{
+      public :
+
+           std::string FormulaName() const { return "TopoDY";}
+
+           std::vector<std::string>  VNamesUnknowns()  const
+           {
+                //  Instrument pose with 6 unknowns : 3 for centers, 3 for axiator
+               // target pose with 3 unknowns : 3 for center
+                return  Append(NamesPose("Ci","Wi"),NamesP3("P_to"));
+           }
+
+           std::vector<std::string>    VNamesObs() const
+           {
+                // for the instrument pose, the 3x3 current rotation matrix as "observation/context"
+                // and the measure value
+                return  Append(NamesMatr("mi",cPt2di(3,3)), {"val"} );
+           }
+
+           template <typename tUk>
+                       std::vector<tUk> formula
+                       (
+                          const std::vector<tUk> & aVUk,
+                          const std::vector<tUk> & aVObs
+                       ) const
+           {
+               cPoseF<tUk>  aPoseInstr(aVUk,0,aVObs,0,true);
+               cPtxd<tUk,3> aP_to = VtoP3(aVUk,6);
+               auto       val = aVObs[9];
+               cPtxd<tUk,3>  aP_to_instr = aPoseInstr.Inverse().Value(aP_to);
+
+               return {  aP_to_instr.y() - val };
+           }
+};
+
+
+class cFormulaTopoDZ
+{
+      public :
+
+           std::string FormulaName() const { return "TopoDZ";}
+
+           std::vector<std::string>  VNamesUnknowns()  const
+           {
+                //  Instrument pose with 6 unknowns : 3 for centers, 3 for axiator
+               // target pose with 3 unknowns : 3 for center
+                return  Append(NamesPose("Ci","Wi"),NamesP3("P_to"));
+           }
+
+           std::vector<std::string>    VNamesObs() const
+           {
+                // for the instrument pose, the 3x3 current rotation matrix as "observation/context"
+                // and the measure value
+                return  Append(NamesMatr("mi",cPt2di(3,3)), {"val"} );
+           }
+
+           template <typename tUk>
+                       std::vector<tUk> formula
+                       (
+                          const std::vector<tUk> & aVUk,
+                          const std::vector<tUk> & aVObs
+                       ) const
+           {
+               cPoseF<tUk>  aPoseInstr(aVUk,0,aVObs,0,true);
+               cPtxd<tUk,3> aP_to = VtoP3(aVUk,6);
+               auto       val = aVObs[9];
+               cPtxd<tUk,3>  aP_to_instr = aPoseInstr.Inverse().Value(aP_to);
+
+               return {  aP_to_instr.z() - val };
+           }
+};
+
+
 };
 
 

diff --git a/MMVII/src/SymbDerGen/GenerateCodes.cpp b/MMVII/src/SymbDerGen/GenerateCodes.cpp
@@ -381,6 +381,40 @@ cCalculator<double> * EqTopoDist(bool WithDerive,int aSzBuf)
     return TplEqTopoDist<double>(WithDerive,aSzBuf);
 }
 
+// topo dX
+template <class Type> cCalculator<Type> * TplEqTopoDX(bool WithDerive,int aSzBuf)
+{
+    return StdAllocCalc(NameFormula(cFormulaTopoDX(),WithDerive),aSzBuf);
+}
+
+cCalculator<double> * EqTopoDX(bool WithDerive,int aSzBuf)
+{
+    return TplEqTopoDX<double>(WithDerive,aSzBuf);
+}
+
+// topo dY
+template <class Type> cCalculator<Type> * TplEqTopoDY(bool WithDerive,int aSzBuf)
+{
+    return StdAllocCalc(NameFormula(cFormulaTopoDY(),WithDerive),aSzBuf);
+}
+
+cCalculator<double> * EqTopoDY(bool WithDerive,int aSzBuf)
+{
+    return TplEqTopoDY<double>(WithDerive,aSzBuf);
+}
+
+// topo dZ
+template <class Type> cCalculator<Type> * TplEqTopoDZ(bool WithDerive,int aSzBuf)
+{
+    return StdAllocCalc(NameFormula(cFormulaTopoDZ(),WithDerive),aSzBuf);
+}
+
+cCalculator<double> * EqTopoDZ(bool WithDerive,int aSzBuf)
+{
+    return TplEqTopoDZ<double>(WithDerive,aSzBuf);
+}
+
+
 
 cCalculator<double> * EqSumSquare(int aNb,bool WithDerive,int aSzBuf,bool ReUse)
 {
@@ -768,6 +802,9 @@ int cAppliGenCode::Exe()
        GenCodesFormula((tREAL8*)nullptr,cFormulaTopoHz(),WithDer);
        GenCodesFormula((tREAL8*)nullptr,cFormulaTopoZen(),WithDer);
        GenCodesFormula((tREAL8*)nullptr,cFormulaTopoDist(),WithDer);
+       GenCodesFormula((tREAL8*)nullptr,cFormulaTopoDX(),WithDer);
+       GenCodesFormula((tREAL8*)nullptr,cFormulaTopoDY(),WithDer);
+       GenCodesFormula((tREAL8*)nullptr,cFormulaTopoDZ(),WithDer);
 
        GenCodesFormula((tREAL8*)nullptr,cDeformImHomotethy()       ,WithDer);
 

diff --git a/MMVII/src/Topo/Topo.cpp b/MMVII/src/Topo/Topo.cpp
@@ -28,6 +28,9 @@ cBA_Topo::cBA_Topo
         {eTopoObsType::eDist, EqTopoDist(true,1)},
         {eTopoObsType::eHz, EqTopoHz(true,1)},
         {eTopoObsType::eZen, EqTopoZen(true,1)},
+        {eTopoObsType::eDX, EqTopoDX(true,1)},
+        {eTopoObsType::eDY, EqTopoDY(true,1)},
+        {eTopoObsType::eDZ, EqTopoDZ(true,1)},
         //{eTopoObsType::eDist, EqDist3D(true,1)},
         //{eTopoObsType::eSubFrame, EqTopoSubFrame(true,1)},
         //{eTopoObsType::eDistParam, EqDist3DParam(true,1)},

diff --git a/MMVII/src/Topo/ctopodata.cpp b/MMVII/src/Topo/ctopodata.cpp
@@ -226,8 +226,8 @@ cTopoData cTopoData::createEx1()
     cTopoObsData aObs2 = {eTopoObsType::eDist, {"ptB", "ptD"},  {10.}, {WW}};
     cTopoObsData aObs3 = {eTopoObsType::eDist, {"ptC", "ptD"},  {10.}, {WW}};
     cTopoObsData aObs4 = {eTopoObsType::eDist, {"ptC", "ptD"},  {10+WW}, {WW}};*/
-    cTopoObsData aObs1 = {eTopoObsType::eDist, {"ptA", "ptD"},  {10.}, {WW}}; // can be inverted
-    cTopoObsData aObs2 = {eTopoObsType::eDist, {"ptD", "ptB"},  {10.}, {WW}}; // can't be inverted
+    cTopoObsData aObs1 = {eTopoObsType::eDist, {"ptA", "ptD"},  {10.}, {WW}};
+    cTopoObsData aObs2 = {eTopoObsType::eDist, {"ptD", "ptB"},  {10.}, {WW}}; // error when previous LinearConstraint bug
     cTopoObsData aObs3 = {eTopoObsType::eDist, {"ptC", "ptD"},  {10.}, {WW}};
     cTopoObsData aObs4 = {eTopoObsType::eDist, {"ptC", "ptD"},  {10+WW}, {WW}};
     /*cTopoObsData aObs1 = {eTopoObsType::eDist, {"ptD", "ptA"},  {10.}, {WW}};

diff --git a/MMVII/src/Topo/ctopodata.h b/MMVII/src/Topo/ctopodata.h
@@ -72,6 +72,9 @@ enum class eCompObsTypes
         eCompDist=3,
         eCompHz=5,
         eCompZen=6,
+        eCompDX=14,
+        eCompDY=15,
+        eCompDZ=4,
         //eSubFrame=11,
         //eDistParam=22,
 };

diff --git a/MMVII/src/Topo/ctopoobs.cpp b/MMVII/src/Topo/ctopoobs.cpp
@@ -24,6 +24,9 @@ cTopoObs::cTopoObs(cTopoObsSet* set, cBA_Topo *aBA_Topo, eTopoObsType type, cons
     case eTopoObsType::eHz:
     case eTopoObsType::eZen:
     case eTopoObsType::eDist:
+    case eTopoObsType::eDX:
+    case eTopoObsType::eDY:
+    case eTopoObsType::eDZ:
         MMVII_INTERNAL_ASSERT_strong(mSet->getType()==eTopoObsSetType::eStation, "Obs: incorrect set type")
         MMVII_INTERNAL_ASSERT_strong(ptsNames.size()==2, "Obs: incorrect number of points")
         MMVII_INTERNAL_ASSERT_strong(measures.size()==1, "Obs: 1 value should be given")

diff --git a/MMVII/src/Topo/ctopoobsset.cpp b/MMVII/src/Topo/ctopoobsset.cpp
@@ -126,7 +126,10 @@ void cTopoObsSetStation::PutUknowsInSetInterval()
 
 void cTopoObsSetStation::createAllowedObsTypes()
 {
-    mAllowedObsTypes = {eTopoObsType::eDist,eTopoObsType::eHz,eTopoObsType::eZen};
+    mAllowedObsTypes = {
+        eTopoObsType::eDist,eTopoObsType::eHz,eTopoObsType::eZen,
+        eTopoObsType::eDX,eTopoObsType::eDY,eTopoObsType::eDZ
+    };
 }