diff --git a/MMVII/Doc/CommandReferences/SysCo.tex b/MMVII/Doc/CommandReferences/SysCo.tex index 394999a571..2a9bff3ce4 100644 --- a/MMVII/Doc/CommandReferences/SysCo.tex +++ b/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 diff --git a/MMVII/Doc/Programmer/NonLinearOptim.tex b/MMVII/Doc/Programmer/NonLinearOptim.tex index 0307cebf7e..47c0a24317 100755 --- a/MMVII/Doc/Programmer/NonLinearOptim.tex +++ b/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 diff --git a/MMVII/src/SymbDerGen/Formulas_Topo.h b/MMVII/src/SymbDerGen/Formulas_Topo.h index 343152edef..9f1b6c03da 100644 --- a/MMVII/src/SymbDerGen/Formulas_Topo.h +++ b/MMVII/src/SymbDerGen/Formulas_Topo.h @@ -258,6 +258,20 @@ class cFormulaTopoDist }; +template +tUk geoc2h(cPtxd 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 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 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 @@ -287,55 +302,22 @@ class cFormulaTopoDH const std::vector & 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 RTL_R = cMatF(3,3,aVObs, 0); + auto RTL_T = VtoP3(aVObs,9); + cPoseF aRTL2Geoc(RTL_T, RTL_R); - cMatF 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 }; } }; diff --git a/MMVII/src/Topo/ctopoobs.cpp b/MMVII/src/Topo/ctopoobs.cpp index dc5377ff2b..be9bf8208c 100644 --- a/MMVII/src/Topo/ctopoobs.cpp +++ b/MMVII/src/Topo/ctopoobs.cpp @@ -146,8 +146,6 @@ std::vector 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(); @@ -155,6 +153,13 @@ std::vector cTopoObs::getVals() const 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 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;