full dH formula

MMVII/Doc/CommandReferences/SysCo.tex

@@ -209,6 +209,7 @@ \section{\texttt{OBS} file format}
 
 \begin{itemize}
     \item \textbf{3}: 3D distance
+    \item \textbf{4}: height difference
     \item \textbf{5}: local horizontal (hz) angle
     \item \textbf{6}: local zenithal (zen) angle
     \item \textbf{7}: local horizontal angle for a new station 

MMVII/Doc/Programmer/NonLinearOptim.tex

@@ -1490,8 +1490,6 @@ \subsubsection{Topo survey equations}
 $$h=\sqrt{ X^2 + Y^2} \cdot \cos \phi + Z \sin \phi - a \sqrt{1-e^2 \sin^2 \phi}$$
 
 Where $a$ and $e$ are constants from the ellipsoid.
-To simplify the computation, $\phi$ can be computed via the SysCo system
-and supposed a constant for derivation, as $a \sqrt{1-e^2 \sin^2 \phi}$ .
 
 \begin{figure}[!h]
 \centering

MMVII/src/SymbDerGen/Formulas_Topo.h

@@ -258,6 +258,20 @@ class cFormulaTopoDist
 };
 
 
+template <typename tUk>
+tUk geoc2h(cPtxd<tUk,3> aP, tUk a, tUk e2) //< Compute ellips height from geocentric coords
+{
+    auto f = 1.-sqrt(1.-e2);
+    auto R = Norm2(aP);
+    // auto lambda = ATan2(aP.y(), aP.x());
+    auto R2d = sqrt(aP.x()*aP.x()+aP.y()*aP.y());
+    auto mu = ATan2(aP.z()*((1.-f)+(e2*a)/R), R2d);
+    auto phi = ATan2(aP.z()*(1.-f)+e2*a*sin(mu)*sin(mu)*sin(mu),
+                     (1.-f)*(R2d-e2*a*cos(mu)*cos(mu)*cos(mu)));
+    auto h = (R2d*cos(phi)) + (aP.z()*sin(phi)) - (a*sqrt(1.-e2*sin(phi)*sin(phi)));
+    return h;
+}
+
 class cFormulaTopoDH
 {
       public :
@@ -266,18 +280,19 @@ class cFormulaTopoDH
 
            std::vector<std::string>  VNamesUnknowns()  const
            {
-               //  Instrument pose with 3 unknowns : 3 for center
-              // target pose with 3 unknowns : 3 for center
-               return  Append(NamesP3("P_from"),NamesP3("P_to"));
+                // origin pt : 3 for center
+                // target pt with 3 unknowns : 3 for center
+                return  Append(NamesP3("P_from"),NamesP3("P_to"));
            }
 
            std::vector<std::string>    VNamesObs() const
            {
-                // RTL to GeoC transfo, as matrix + translation
-                // GeoG phi (in rad) and M (=a*sqrt(1-e*e*sin(phi)*sin(phi))) for each point
-                // measured value
-                return  Append(NamesMatr("M_RTL",cPt2di(3,3)), NamesP3("T_RTL"),
-                               {"Phi_from", "M_from", "Phi_to", "M_to", "val"});
+                // RTL2geoc rot + tr
+                // a and e2 from ellipsoid
+                // and the measure value
+                return  Append(NamesMatr("RTL2Geoc_R",cPt2di(3,3)),
+                               NamesP3("RTL2Geoc_T"),
+                               {"a", "e2", "val"});
            }
 
            template <typename tUk>
@@ -287,55 +302,22 @@ class cFormulaTopoDH
                           const std::vector<tUk> & aVObs
                        ) const
            {
+               auto aP_from_RTL = VtoP3(aVUk,0);
+               auto aP_to_RTL   = VtoP3(aVUk,3);
+               auto           a = aVObs[12];
+               auto          e2 = aVObs[13];
+               auto         val = aVObs[14];
+               cMatF<tUk> RTL_R = cMatF(3,3,aVObs, 0);
+               auto       RTL_T = VtoP3(aVObs,9);
+               cPoseF aRTL2Geoc(RTL_T, RTL_R);
 
-               cMatF<tUk> M_RTL = cMatF(3, 3, aVObs, 0);
-               auto T_RTL = VtoP3(aVObs,9);
-               auto Phi_from = aVObs[12];
-               auto M_from = aVObs[13];
-               auto Phi_to = aVObs[14];
-               auto M_to = aVObs[15];
-               auto val = aVObs[16];
+               auto aP_from_geoc = aRTL2Geoc.Value(aP_from_RTL);
+               auto aP_from_h = geoc2h(aP_from_geoc, a, e2);
 
-               // convert points to GeoC
-               auto aP_from_RTL = VtoP3(aVUk,0);
-               auto aP_from_GeoC =  M_RTL * aP_from_RTL + T_RTL;
-               auto aP_to_RTL = VtoP3(aVUk,3);
-               auto aP_to_GeoC =  M_RTL * aP_to_RTL + T_RTL;
-
-               // convert GeoC to GeoG
-               /* // Iterative formula
-               auto p_from = sqrt(aP_from_GeoC.x()*aP_from_GeoC.x()
-                             +aP_from_GeoC.y()*aP_from_GeoC.y());
-               auto h_from = p_from/cos(Phi_from) - N_from;
-
-               auto p_to = sqrt(aP_to_GeoC.x()*aP_to_GeoC.x()
-                             +aP_to_GeoC.y()*aP_to_GeoC.y());
-               auto h_to = p_to/cos(Phi_to) - N_to; */
-
-               // Bowring, 1985, The accuracy of geodetic latitude and height equations
-               // https://geodesie.ign.fr/contenu/fichiers/documentation/pedagogiques/TransformationsCoordonneesGeodesiques.pdf
-               auto p_from = sqrt(aP_from_GeoC.x()*aP_from_GeoC.x()
-                             +aP_from_GeoC.y()*aP_from_GeoC.y());
-               auto h_from = p_from*cos(Phi_from) + aP_from_GeoC.z()*sin(Phi_from) - M_from;
-
-               auto p_to = sqrt(aP_to_GeoC.x()*aP_to_GeoC.x()
-                             +aP_to_GeoC.y()*aP_to_GeoC.y());
-               auto h_to = p_to*cos(Phi_to) + aP_to_GeoC.z()*sin(Phi_to) - M_to;
-
-               auto dH = h_to - h_from;
-
-               /*
-               SymbPrint(aP_from_GeoC.x(),"aP_from_GeoC.x");
-               SymbPrint(aP_from_GeoC.y(),"aP_from_GeoC.y");
-               SymbPrint(aP_from_GeoC.z(),"aP_from_GeoC.z");
-               SymbPrint(aP_to_GeoC.x(),"aP_to_GeoC.x");
-               SymbPrint(aP_to_GeoC.y(),"aP_to_GeoC.y");
-               SymbPrint(aP_to_GeoC.z(),"aP_to_GeoC.z");
-               SymbPrint(h_from,"h_from");
-               SymbPrint(h_to,"h_to");
-               */
-
-               return {  dH - val };
+               auto aP_to_geoc = aRTL2Geoc.Value(aP_to_RTL);
+               auto aP_to_h = geoc2h(aP_to_geoc, a, e2);
+
+               return {  aP_to_h - aP_from_h  - val };
            }
 };
 

MMVII/src/Topo/ctopoobs.cpp

@@ -146,15 +146,20 @@ std::vector<tREAL8> cTopoObs::getVals() const
             MMVII_INTERNAL_ERROR("error set type")
             return {}; //just to please the compiler
         }
-        cPt3dr* aPtFrom = mBA_Topo->getPoint(mPtsNames[0]).getPt();
-        cPt3dr* aPtTo = mBA_Topo->getPoint(mPtsNames[1]).getPt();
         if (mType==eTopoObsType::eDH)
         {
             auto aSysCo = mBA_Topo->getSysCo();
             // RTL to GeoC transfo, as matrix + translation
             const tPoseR* aTranfo2GeoC = aSysCo->getTranfo2GeoC();
             aTranfo2GeoC->Rot().Mat().PushByLine(vals); // TODO: why by line?
             aTranfo2GeoC->Tr().PushInStdVector(vals);
+            // a
+            vals.push_back(aSysCo->getEllipsoid_a());
+            // e2
+            vals.push_back(aSysCo->getEllipsoid_e2());
+            /*
+            cPt3dr* aPtFrom = mBA_Topo->getPoint(mPtsNames[0]).getPt();
+            cPt3dr* aPtTo = mBA_Topo->getPoint(mPtsNames[1]).getPt();
             //Phi_from
             auto aPtFromGeoG = aSysCo->toGeoG(*aPtFrom);
             auto aPhiFrom = aPtFromGeoG.y()/AngleInRad(eTyUnitAngle::eUA_degree);
@@ -170,7 +175,7 @@ std::vector<tREAL8> cTopoObs::getVals() const
             //M_To = a*sqrt(1-e*e*sin(phi)*sin(phi))
             auto aPtToM = aSysCo->getEllipsoid_a()
                     *sqrt(1-aSysCo->getEllipsoid_e2()*sin(aPhiTo)*sin(aPhiTo));
-            vals.push_back(aPtToM);
+            vals.push_back(aPtToM);*/
         }
         vals.insert(std::end(vals), std::begin(mMeasures), std::end(mMeasures));
         break;