diff --git a/MMVII/Doc/CommandReferences/SysCo.tex b/MMVII/Doc/CommandReferences/SysCo.tex index c91661d515..c03a9f8cc4 100644 --- a/MMVII/Doc/CommandReferences/SysCo.tex +++ b/MMVII/Doc/CommandReferences/SysCo.tex @@ -166,7 +166,8 @@ \section{Survey introduction} \item direct Euclidian vector observation \end{itemize} -The measurements can be made between cameras poses, GCPs or undeclared points that will be inserted into the last GCP set of the adjustment. +The measurements can be made between cameras poses, GCPs or undeclared points that will be inserted into the last GCP set of the adjustment +(if automatic coordinates initialization succeeds). Two \CdPPP\ commands can use topo measurements in compensation: \begin{itemize} @@ -174,7 +175,7 @@ \section{Survey introduction} \item \texttt{TopoAdj} (see \ref{subsec:TopoAdj}) \end{itemize} -The topo measurements file can be given as a set of \CdPPP\ json or xml file, or in a simplified text format (named \texttt{OBS} file) inherited from IGN's +The topo measurements files can be given as a set of \CdPPP\ json or xml files, or in a simplified text format (named \texttt{OBS} file) inherited from IGN's Comp3D\footnote{\url{https://github.com/IGNF/Comp3D}} micro-geodesy compensation software. All the measurements files must be in the \texttt{MMVII-PhgrProj/Topo/[TopoObsName]} folder. @@ -248,7 +249,7 @@ \subsection{3D points file} To import these coordinates in \CdPPP\ we use the \texttt{ImportGCP} command, where we give the text format (additional\_info, name, x, y, z), the name of the resulting \texttt{PointsMeasure} and the coordinates SysCo. -We also specify that the points who have an additional\_info of '0' are free points, that the sigma for +We also specify that the points that have '0' for their additional\_info are free points, that the sigma for known points is 0.001m and that lines starting with '*' are comment lines. \begin{lstlisting} @@ -256,7 +257,7 @@ \subsection{3D points file} \end{lstlisting} In the resulting file \texttt{MMVII-PhgrProj/PointsMeasure/InitL93/MesGCP-coords.xml}, -the points PtA and PtD have a \texttt{\_\_Opt\_\_Sigma2} set to 0.001 m $\sigma$, +the points PtA and PtD have a set \texttt{\_\_Opt\_\_Sigma2} equivalent to $\sigma = 0.001 m$ , the points PtB and PtC have no \texttt{\_\_Opt\_\_Sigma2}, making them free points. @@ -387,22 +388,51 @@ \subsection{Adjustment} \section{Stations orientation constraints} -Each station has orientation constraints that have to be given before the station observations lines in the \texttt{{obs}} file. +Each station has orientation constraints that have to be given before the station observations lines in the \texttt{OBS} file. +Each orientation constraint is indicated as a line starting with \texttt{\#} (from now on, called a \texttt{\#}-line). +\\ The possible orientation constraints are: \begin{itemize} \item \texttt{\#FIX}: the station is axis-aligned, it is verticalized and oriented to North \item \texttt{\#VERT}: the station is verticalized and only horizontal orientation is free \item \texttt{\#BASC}: the station orientation has 3 degrees of freedom, meaning non-verticalized and not oriented to North + \item TODO \texttt{\#CAM}:the station orientation is the camera orientation (only allowed for point names that correspond to image file names) \end{itemize} -After a \texttt{\#} line (\texttt{\#FIX}, \texttt{\#VERT} or \texttt{\#BASC}), all the following stations have the new orientation constraint until the next \texttt{\#} line. +After a \texttt{\#}-line, all the following stations have the new orientation constraint until the next \texttt{\#}-line with new orientation unknowns. +For convenience, \texttt{\#NEW} can be used to keep current orientation constraint type and just add new unknowns for orientation (see \ref{subsec:SeveralStationsSamePoint}). + Each \texttt{OBS} file starts with an implicit \texttt{\#VERT}, making the stations verticalized by default. For now, the vertical is modeled as the Earth's ellipsoid normal. Vertical deflection grids may be added later. + +\subsection{Frames for survey stations} + +The computation must take place in a 3D euclidian frame. For now, only RTL SysCo is supported. +Figure \ref{fig:topoFrames} shows the frames used in topo compensation. + +\begin{figure}[!h] +\centering +\includegraphics[width=12cm]{Programmer/framesTopo.png} +\caption{The different frames types used in compensation: + \texttt{Green}: a projection SysCo. As it is not euclidian, compensation should not be done in this frame. + \texttt{Blue}: the RTL SysCo for the computation. Every parameter is in this frame. + \texttt{Purple}: each 3D point has its own local euclidian frame tangent to the ellipsoid + \texttt{Yellow}: each topometric station has a rotation between the local euclidian tangent frame and the instrument frame. + } +\label{fig:topoFrames} +\end{figure} + + +Stations orientation constraints fix some parts of the rotation between local tangent frame and instrument frame: +no rotation (\textbf{\#FIX}), rotation around $Z$ only (\textbf{\#VERT}), or free rotation (\textbf{\#BASC}). + + + \subsection{Return to quickstart example} The example \texttt{OBS} file was: \begin{verbatim} @@ -465,20 +495,85 @@ \subsection{Return to quickstart example} \subsection{Several stations on the same point} +\label{subsec:SeveralStationsSamePoint} \CdPPP\ automatically creates a new station when a point is used for the first time as the origin of a measurement. If we have to make a new set of orientation unknowns because two instruments were set on the same point with different -orientations, we can: +orientations, we can either: \begin{itemize} - \item use separated \texttt{OBS} files - \item add a \texttt{\#} line to separate the measurements sets - \item use a code \textbf{7} for the first measurement + \item use separate \texttt{OBS} files + \item add a \texttt{\#}-line to separate the measurements sets (\texttt{\#NEW} is useful in this case to make a station with the same orientation constraints as before) + \item use a code \textbf{7} instead of \textbf{5} for the first measurement of the new station \end{itemize} +A separate \texttt{OBS} files or a \texttt{\#}-line closes all current stations. +Code \textbf{7} only closes the previous station on one point. + +Some illustrations : +\begin{verbatim} + 7 St1 PtA 100.000 0.001 * creates a station on St1 + 5 St1 PtB 110.000 0.001 + + 7 St1 PtA 150.000 0.001 * closes current station on St1, creates a new one on St1 + 5 St1 PtC 210.000 0.001 +\end{verbatim} +Creates two stations originating on point \texttt{St1}. Each of these station +is vericalized (thanks to the implicit \texttt{\#VERT} at the start of the file) and each has one horizontal orientation unknown. +Here, there is a 50 grades difference between the two stations orientations. + +With: +\begin{verbatim} + 7 St1 PtA 100.000 0.001 * creates a station on St1 + 5 St1 PtB 110.000 0.001 + + 5 St1 PtA 150.000 0.001 + 5 St1 PtC 210.000 0.001 +\end{verbatim} +There is only one station on \texttt{St1}. The incoherent horizontal angles values +will make the computation fail. + +Another way to fix it: +\begin{verbatim} + 5 St1 PtA 100.000 0.001 * creates a station on St1 + 5 St1 PtB 110.000 0.001 +#NEW * closes all current stations + 5 St1 PtA 150.000 0.001 * creates a station on St1 + 5 St1 PtC 210.000 0.001 +\end{verbatim} +The fisrt \textbf{7} is not mandatory since it is the start of a file, there is no +current stations. \texttt{\#NEW} closes all the current stations, keeping the same orientations constraints (stations verticalized). + +Code \textbf{7} only closes the current station on one point : +\begin{verbatim} + 5 St1 PtA 100.000 0.001 * creates a station on St1 + 5 St1 PtB 110.000 0.001 + + 5 St2 PtA 200.000 0.001 * creates a station on St2 + 5 St2 PtD 300.000 0.001 + + 7 St1 PtA 150.000 0.001 * closes station on St1, creates a new one on St1 + 5 St1 PtC 210.000 0.001 + + 5 St2 PtE 250.000 0.001 * uses previous station on St2 +\end{verbatim} +But: +\begin{verbatim} + 5 St1 PtA 100.000 0.001 * creates a station on St1 + 5 St1 PtB 110.000 0.001 + + 5 St2 PtA 200.000 0.001 * creates a station on St2 + 5 St2 PtD 300.000 0.001 +#NEW * closes all the stations + 5 St1 PtA 150.000 0.001 * creates a station on St1 + 5 St1 PtC 210.000 0.001 + + 5 St2 PtE 250.000 0.001 * creates a station on St2 +\end{verbatim} + -\subsection{Special case of distance-only measurements} +\subsection{Special case for distance-only measurements} If a station only has distances measurements, it is automatically set as a \texttt{\#FIX} station, since this station orientation unknowns diff --git a/MMVII/Doc/Programmer/NonLinearOptim.tex b/MMVII/Doc/Programmer/NonLinearOptim.tex index 3793b3ccce..13c7460da2 100755 --- a/MMVII/Doc/Programmer/NonLinearOptim.tex +++ b/MMVII/Doc/Programmer/NonLinearOptim.tex @@ -1315,36 +1315,25 @@ \section{Survey measurements} \label{Chap:TopoProg} \subsection{Global presentation} -The premises of a topographic survey compensation system can be found in \texttt{MMVII/src/Topo/}. +Topographic survey compensation system can be found in \texttt{MMVII/src/Topo/}. -For now it is used in commands \texttt{OriBundleAdj} and \texttt{TopoAdj}, and in the Bench \texttt{TopoComp}. +For now it is used in commands \texttt{OriBundleAdj} and \texttt{TopoAdj}, and in the Bench \texttt{TopoComp} (see \ref{Chap:TopoUser}). This Bench will be used to illustrate the survey classes and their usage. -\subsection{Frames for survey stations} - -\begin{figure}[!h] -\centering -\includegraphics[width=12cm]{Programmer/framesTopo.png} -\caption{The different frames types used in compensation} -\label{fig:topoFrames} -\end{figure} - +\subsubsection{Topo survey classes} +The topo files are: \begin{itemize} - \item \texttt{Green}: a projection SysCo. As it is not euclidian, all point coordinates must be converted into RTL before compensation. - \item \texttt{Blue}: the RTL SysCo for the computation. Every parameter is in this frame. - \item \texttt{Purple}: each point has its own local euclidian tangent frame depending on its position. - \item \texttt{Yellow}: each topoemtric station has a rotation between the local euclidian tangent frame and the instrument frame. + \item \texttt{src/Topo/ctopopoint.h, src/Topo/ctopopoint.cpp}: class \texttt{cTopoPoint} (see below) + \item \texttt{src/Topo/ctopoobs.h, src/Topo/ctopoobs.cpp}: class \texttt{cTopoObs} (see below) + \item \texttt{src/Topo/ctopoobsset.h, src/Topo/ctopoobsset.cpp}: classes \texttt{cTopoObsSet} and derived classes (see below) + \item \texttt{src/Topo/topoinit.h, src/Topo/topoinit.cpp}: coordinates initialisation algorithms + \item \texttt{src/Topo/ctopodata.h, src/Topo/ctopodata.cpp}: classes for data serialization and deserialization + \item \texttt{include/MMVII\_Topo.h, src/Topo/Topo.cpp}: class \texttt{cBA\_Topo} (see below) \end{itemize} -The computation must take place in a 3D euclidian frame. -For now, only RTL SysCo is supported. - - -\subsubsection{Topo survey classes} - The topo computation classes are : \begin{itemize} \item \texttt{cTopoPoint}: a point used with survey measurements. Keeps a pointer to the unknowns from GCP or Ori. @@ -1363,6 +1352,17 @@ \subsubsection{Topo survey classes} It has a notion of {\tt origin} (the point where the instrument is positionned), and an orientation matrix in the RTL SysCo. If the station is verticalized, the initial orientation matrix corresponds to the vertical orientation on the origin point, and only a rotation arround the vertical direction is free. +\subsubsection{Integration into photogrammetric compensation} + +Each topo measurement is between 3D points corresponding to system unknowns. These 3D points can be camera origins or GCPs. + +It is allowed to have topo measurements referencing points that are not cameras origins nor declared in GCP list. +In this case, new 3D points will be created, and added to the last GCP set of the adjustment (during the last \texttt{cAppliBundlAdj::AddOneSetGCP()} call). + +Those points will have no coordinates constraints (free), and will be initialized by the topo system. If there is no initialization method usable for the point, +an error will occure and the user will have to provide initial coordinates in the GCP 3D file. + + \subsubsection{Topo survey equations} For \texttt{cTopoObsSetStation}, all the measurements are expressed in the local survey station frame. diff --git a/MMVII/include/MMVII_MeasuresIm.h b/MMVII/include/MMVII_MeasuresIm.h index 0213819e47..4400980417 100755 --- a/MMVII/include/MMVII_MeasuresIm.h +++ b/MMVII/include/MMVII_MeasuresIm.h @@ -144,6 +144,7 @@ class cMes1GCP std::string mAdditionalInfo; std::optional > mOptSigma2; // xx xy xz yy yz zz + bool isInit() const {return mPt.IsValid();} }; /** A set of cMes1GCP */ diff --git a/MMVII/src/Topo/Topo.h b/MMVII/include/MMVII_Topo.h similarity index 87% rename from MMVII/src/Topo/Topo.h rename to MMVII/include/MMVII_Topo.h index 7100e8fe14..2c9f75c6d1 100644 --- a/MMVII/src/Topo/Topo.h +++ b/MMVII/include/MMVII_Topo.h @@ -1,7 +1,13 @@ +#ifndef TOPO_H +#define TOPO_H + #include "MMVII_SysSurR.h" #include "MMVII_Sensor.h" #include "MMVII_SysCo.h" -#include "ctopodata.h" +#include "../src/Topo/ctopopoint.h" +#include "../src/Topo/ctopoobs.h" +#include "../src/Topo/ctopoobsset.h" +#include "../src/Topo/ctopodata.h" using namespace NS_SymbolicDerivative; @@ -11,16 +17,14 @@ namespace MMVII class cMMVII_BundleAdj; class cPhotogrammetricProject; class cSensorCamPC; -class cTopoObsSet; -class cTopoObs; -class cTopoPoint; class cBA_GCP; +typedef std::map> tStationsMap; + class cBA_Topo : public cMemCheck { friend class cTopoData; public : - cBA_Topo(cPhotogrammetricProject *aPhProj, cMMVII_BundleAdj *aBA); ~cBA_Topo(); void clear(); @@ -42,6 +46,9 @@ public : double Sigma0() {return mSigma0;} std::vector GetObsPoint(std::string aPtName) const; + bool tryInitAll(); + bool tryInit(cTopoPoint & aTopoPt, tStationsMap & stationsMap); + bool mergeUnknowns(cResolSysNonLinear &aSys); //< if several stations share origin etc. void makeConstraints(cResolSysNonLinear &aSys); const std::map & getAllPts() const { return mAllPts; } @@ -65,3 +72,4 @@ private : }; +#endif // TOPO_H diff --git a/MMVII/include/MMVII_enums.h b/MMVII/include/MMVII_enums.h index 0cdc52fb32..097d1334a5 100755 --- a/MMVII/include/MMVII_enums.h +++ b/MMVII/include/MMVII_enums.h @@ -569,10 +569,11 @@ enum class eTopoObsType // cTopoObsSetStation orientation freedom status enum class eTopoStOriStat { - eTopoStOriFixed, ///< no rotation - eTopoStOriVert, ///< z rotation - eTopoStOriBasc, ///< 3d rotation - eNbVals ///< Tag for number of value + eTopoStOriContinue, ///< special case, used only on obs reading: same as previous ori constraint, just a marker to split stations + eTopoStOriFixed, ///< no rotation + eTopoStOriVert, ///< z rotation + eTopoStOriBasc, ///< 3d rotation + eNbVals ///< Tag for number of value }; diff --git a/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp b/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp index 8b8fe7bbc0..37fd997fb7 100644 --- a/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp +++ b/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp @@ -4,7 +4,7 @@ \file cAppliBundAdj.cpp */ -#include "../Topo/Topo.h" +#include "MMVII_Topo.h" /* Track info on bundle adj/push broom in V1 : diff --git a/MMVII/src/BundleAdjustment/cAppliTopoAdj.cpp b/MMVII/src/BundleAdjustment/cAppliTopoAdj.cpp index b420785e3b..6efa39b51e 100644 --- a/MMVII/src/BundleAdjustment/cAppliTopoAdj.cpp +++ b/MMVII/src/BundleAdjustment/cAppliTopoAdj.cpp @@ -4,7 +4,7 @@ \file cAppliTopoAdj.cpp */ -#include "../Topo/Topo.h" +#include "MMVII_Topo.h" @@ -169,9 +169,6 @@ int cAppliTopoAdj::Exe() mBA.OneIterationTopoOnly(mLVM, true); } - for (auto & aSI : mBA.VSIm()) - mPhProj.SaveSensor(*aSI); - mBA.Save_newGCP(); mBA.SaveTopo(); // just for debug for now diff --git a/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp b/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp index 6588ae8017..7e7123f95e 100644 --- a/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp +++ b/MMVII/src/BundleAdjustment/cMMVII_BundleAdj.cpp @@ -1,8 +1,7 @@ #include "BundleAdjustment.h" #include "MMVII_util_tpl.h" -#include "../Topo/Topo.h" -#include "../Topo/ctopoobs.h" +#include "MMVII_Topo.h" /** \file cAppliBundAdj.cpp diff --git a/MMVII/src/Sensors/Measures.cpp b/MMVII/src/Sensors/Measures.cpp index 9be175a8b5..0dd3401654 100644 --- a/MMVII/src/Sensors/Measures.cpp +++ b/MMVII/src/Sensors/Measures.cpp @@ -528,7 +528,7 @@ cMes1GCP::cMes1GCP(const cPt3dr & aPt, const std::string & aNamePt, tREAL4 aSigm } cMes1GCP::cMes1GCP() : - cMes1GCP (cPt3dr(0,0,0),"??") + cMes1GCP (cPt3dr::Dummy(),"??") { } diff --git a/MMVII/src/Sensors/cPhotogrammetricProject.cpp b/MMVII/src/Sensors/cPhotogrammetricProject.cpp index dedc046f55..cb09567d99 100644 --- a/MMVII/src/Sensors/cPhotogrammetricProject.cpp +++ b/MMVII/src/Sensors/cPhotogrammetricProject.cpp @@ -8,7 +8,7 @@ #include "MMVII_DeclareCste.h" #include "MMVII_Clino.h" #include "cExternalSensor.h" -#include "../src/Topo/Topo.h" // TODOJM +#include "MMVII_Topo.h" /** diff --git a/MMVII/src/Serial/ElemStrToVal.cpp b/MMVII/src/Serial/ElemStrToVal.cpp index acd604da3d..34070d4cd1 100644 --- a/MMVII/src/Serial/ElemStrToVal.cpp +++ b/MMVII/src/Serial/ElemStrToVal.cpp @@ -193,6 +193,7 @@ template<> cE2Str::tMapE2Str cE2Str::mE2S template<> cE2Str::tMapE2Str cE2Str::mE2S { + {eTopoStOriStat::eTopoStOriContinue,"#NEW"}, {eTopoStOriStat::eTopoStOriFixed,"#FIX"}, {eTopoStOriStat::eTopoStOriVert,"#VERT"}, {eTopoStOriStat::eTopoStOriBasc,"#BASC"}, diff --git a/MMVII/src/Topo/Topo.cpp b/MMVII/src/Topo/Topo.cpp index 11d271db02..8872514362 100644 --- a/MMVII/src/Topo/Topo.cpp +++ b/MMVII/src/Topo/Topo.cpp @@ -1,8 +1,6 @@ -#include "Topo.h" +#include "MMVII_Topo.h" #include "MMVII_PhgrDist.h" -#include "ctopopoint.h" -#include "ctopoobsset.h" -#include "ctopoobs.h" +#include "topoinit.h" #include "../BundleAdjustment/BundleAdjustment.h" #include "cMMVII_Appli.h" #include @@ -76,9 +74,9 @@ void cBA_Topo::clear() void cBA_Topo::findPtsUnknowns(const std::vector & vGCP, cPhotogrammetricProject *aPhProj) { - for (auto & [aName, aPtT] : getAllPts()) + for (auto & [aName, aTopoPt] : getAllPts()) { - aPtT.findUK(vGCP, aPhProj, aPtT.getInitCoord()); + aTopoPt.findUK(vGCP, aPhProj, aTopoPt.getInitCoord()); } } @@ -146,7 +144,7 @@ void cBA_Topo::AddPointsFromDataToGCP(cSetMesImGCP &aFullMesGCP, std::vector & vGCP, cPhotogrammetricPro { findPtsUnknowns(vGCP, aPhProj); - // finish initialization when points are ready + // initialization + tryInitAll(); + + // check that everything is initialized + std::string aPtsNamesUninit=""; + for (auto& [aName, aTopoPt] : mAllPts) + { + if (!aTopoPt.isInit()) + aPtsNamesUninit += aName + " "; + } + MMVII_INTERNAL_ASSERT_User(aPtsNamesUninit.empty(), eTyUEr::eUnClassedError, + "Error: Initialization has failed for points: "+aPtsNamesUninit) for (auto & aSet: mAllObsSets) { - MMVII_INTERNAL_ASSERT_User(aSet->initialize(), eTyUEr::eUnClassedError, - "Error: Station initialization failed.") + MMVII_INTERNAL_ASSERT_User(aSet->isInit(), eTyUEr::eUnClassedError, + "Error: Obs Set initialization failed.") } mIsReady = true; } @@ -169,8 +178,8 @@ void cBA_Topo::FromData(const std::vector & vGCP, cPhotogrammetricPro void cBA_Topo::print() { StdOut() << "Points:\n"; - for (auto& [aName, aPtT] : mAllPts) - StdOut() << " - "<toString()<<"\n"; @@ -299,7 +308,70 @@ void cBA_Topo::AddTopoEquations(cResolSysNonLinear & aSys) mSigma0 = sqrt(mSigma0/(aNbObs-aNbUk)); } +bool cBA_Topo::tryInitAll() +{ + // get all stations ordered by origin to optimize research + for (auto & aSet: mAllObsSets) + aSet->initialize(); // to get origin point for stations + tStationsMap allStations; + for (auto & aSet: mAllObsSets) + { + if (aSet->getType() == eTopoObsSetType::eStation) + { + cTopoObsSetStation* set = dynamic_cast(aSet); + if (!set) + MMVII_INTERNAL_ERROR("error set type") + allStations[set->getPtOrigin()].push_back(set); + } + } + int aNbUninit=0; + for (auto & aSet: mAllObsSets) + if (!aSet->isInit()) + ++aNbUninit; + for (auto& [aName, aTopoPt] : mAllPts) + if (!aTopoPt.isInit()) + ++aNbUninit; + int aPreviousNbUninit = aNbUninit + 1; // kickstart + + while (aPreviousNbUninit>aNbUninit) + { +#ifdef VERBOSE_TOPO + StdOut() << "tryInitAll: " << aNbUninit << " to init.\n"; +#endif + + for (auto& [aName, aTopoPt] : mAllPts) + if (!aTopoPt.isInit()) + tryInit(aTopoPt, allStations); + for (auto & aSet: mAllObsSets) + if (!aSet->isInit()) + aSet->initialize(); + + aPreviousNbUninit = aNbUninit; + aNbUninit = 0; + for (auto & aSet: mAllObsSets) + if (!aSet->isInit()) + ++aNbUninit; + for (auto& [aName, aTopoPt] : mAllPts) + if (!aTopoPt.isInit()) + ++aNbUninit; + } + return aNbUninit==0; +} + +bool cBA_Topo::tryInit(cTopoPoint & aPtToInit, tStationsMap &stationsMap) +{ + if (aPtToInit.isInit()) + return true; +#ifdef VERBOSE_TOPO + StdOut() << "tryInit: " << aPtToInit.getName() <<".\n"; +#endif + + return + tryInit3Obs1Station(aPtToInit, stationsMap) + || tryInitVertStations(aPtToInit, stationsMap) + ; +} //------------------------------------------------------------------- diff --git a/MMVII/src/Topo/ctopodata.cpp b/MMVII/src/Topo/ctopodata.cpp index 61a1a7c433..b46eb74ee1 100644 --- a/MMVII/src/Topo/ctopodata.cpp +++ b/MMVII/src/Topo/ctopodata.cpp @@ -1,10 +1,7 @@ #include "ctopodata.h" #include "MMVII_PhgrDist.h" -#include "ctopopoint.h" -#include "ctopoobsset.h" -#include "ctopoobs.h" #include "MMVII_2Include_Serial_Tpl.h" -#include "Topo.h" +#include "MMVII_Topo.h" #include #include #include @@ -181,7 +178,7 @@ bool cTopoData::InsertCompObsFile(const std::string & aFileName) std::ifstream infile(aFileName); if (!infile.is_open()) { - StdOut() << "Error: can't open obs file \""<> code)) { - StdOut() << "Error reading "<> nameFrom >> nameTo >> val >> sigma)) { - StdOut() << "Error reading "< cTopoData::createEx3() cSetMesGCP aSetPts; aSetPts.AddMeasure( cMes1GCP(cPt3dr(100,110,100), "Ori1", 0.001) ); aSetPts.AddMeasure( cMes1GCP(cPt3dr(100,100,100), "St1", 0.001) ); - aSetPts.AddMeasure( cMes1GCP(cPt3dr(105,115,105), "Tr1") ); // 107.072, 107.072, 100 + //aSetPts.AddMeasure( cMes1GCP(cPt3dr(105,115,105), "Tr1") ); // init not needed. Final: 107.072, 107.072, 100 double g0 = 2.2; cTopoObsData aObs1 = {eTopoObsType::eHz, {"St1", "Ori1"}, {0. - g0}, {0.001}}; diff --git a/MMVII/src/Topo/ctopoobs.cpp b/MMVII/src/Topo/ctopoobs.cpp index 97ca97d90d..a0fe535940 100644 --- a/MMVII/src/Topo/ctopoobs.cpp +++ b/MMVII/src/Topo/ctopoobs.cpp @@ -2,10 +2,8 @@ #include "MMVII_PhgrDist.h" #include "MMVII_2Include_Serial_Tpl.h" #include -#include "ctopoobsset.h" -#include "ctopopoint.h" #include "MMVII_SysSurR.h" -#include "Topo.h" +#include "MMVII_Topo.h" namespace MMVII { diff --git a/MMVII/src/Topo/ctopoobsset.cpp b/MMVII/src/Topo/ctopoobsset.cpp index cd16153f0a..70bc42529c 100644 --- a/MMVII/src/Topo/ctopoobsset.cpp +++ b/MMVII/src/Topo/ctopoobsset.cpp @@ -1,17 +1,14 @@ #include "ctopoobsset.h" #include "MMVII_PhgrDist.h" #include "MMVII_2Include_Serial_Tpl.h" -#include "ctopoobs.h" -#include "ctopopoint.h" -#include "ctopodata.h" #include "MMVII_PCSens.h" -#include "Topo.h" +#include "MMVII_Topo.h" #include namespace MMVII { cTopoObsSet::cTopoObsSet(cBA_Topo * aBA_Topo, eTopoObsSetType type): - mType(type), mBA_Topo(aBA_Topo) + mType(type), mBA_Topo(aBA_Topo), mInit(false) { } @@ -158,6 +155,10 @@ void cTopoObsSetStation::makeConstraints(cResolSysNonLinear & aSys) switch (mOriStatus) { + case(eTopoStOriStat::eTopoStOriContinue): + // should not exist, Continue is only for obs files + MMVII_INTERNAL_ASSERT_strong(false, "cTopoObsSetStation::makeConstraints: incorrect ori status") + break; case(eTopoStOriStat::eTopoStOriFixed): mRotOmega.Pt() = {0.,0.,0.}; #ifdef VERBOSE_TOPO @@ -224,24 +225,33 @@ bool cTopoObsSetStation::initialize() } setOrigin(aOriginName); // use 1st from name as station name + mInit = false; // initialize // mRotSysCo2Vert is initialized by setOrigin() switch (mOriStatus) { + case(eTopoStOriStat::eTopoStOriContinue): + // should not exist, Continue is only for obs files + MMVII_INTERNAL_ASSERT_strong(false, "cTopoObsSetStation::initialize: incorrect ori status") + break; case(eTopoStOriStat::eTopoStOriFixed): + mInit = mPtOrigin->isInit(); return true; // nothing to do case(eTopoStOriStat::eTopoStOriVert): { + if (!mPtOrigin->isInit()) + return false; cTopoObs * aObsDX = nullptr; cTopoObs * aObsDY = nullptr; tREAL8 G0 = NAN; for (auto & obs: mObs) { + auto & aPtTo = mBA_Topo->getPoint(obs->getPointName(1)); + if (!aPtTo.isInit()) + continue; if (obs->getType() == eTopoObsType::eHz) { - // TODO: use projection for init G0 - // TODO: check if points are init - auto & aPtTo = mBA_Topo->getPoint(obs->getPointName(1)); + // TODO: use projection for G0 init G0 = atan2( aPtTo.getPt()->x() - mPtOrigin->getPt()->x(), aPtTo.getPt()->y() - mPtOrigin->getPt()->y()) - obs->getMeasures().at(0); @@ -258,7 +268,6 @@ bool cTopoObsSetStation::initialize() if (aObsDX && aObsDY) // compute G0 from DX DY if Hz not found { // TODO: use projection for init G0 - // TODO: check if points are init auto & aPtTo = mBA_Topo->getPoint(aObsDX->getPointName(1)); G0 = atan2( aPtTo.getPt()->x() - mPtOrigin->getPt()->x(), aPtTo.getPt()->y() - mPtOrigin->getPt()->y()) @@ -277,6 +286,7 @@ bool cTopoObsSetStation::initialize() StdOut() << " "<isInit(); return true; case(eTopoStOriStat::eNbVals): MMVII_INTERNAL_ASSERT_strong(false, "cTopoObsSetStation::initialize: incorrect ori status") @@ -310,16 +321,10 @@ void cTopoObsSetStation::setOrigin(std::string _OriginName) mPtOrigin = &mBA_Topo->getPoint(_OriginName); mOriginName = _OriginName; - // automatic origin initialization - if (!mPtOrigin->getPt()->IsValid()) - { - MMVII_DEV_WARNING("cTopoObsSetStation origin initialization not ready.") - *mPtOrigin->getPt() = {0.,0.,0.}; - } - mRotVert2Instr = tRot::Identity(); mRotOmega.Pt() = {0.,0.,0.}; - updateVertMat(); + if (mPtOrigin->isInit()) + updateVertMat(); } tREAL8 cTopoObsSetStation::getG0() const diff --git a/MMVII/src/Topo/ctopoobsset.h b/MMVII/src/Topo/ctopoobsset.h index a18a70467e..66f9213095 100644 --- a/MMVII/src/Topo/ctopoobsset.h +++ b/MMVII/src/Topo/ctopoobsset.h @@ -31,8 +31,9 @@ class cTopoObsSet : public cObjWithUnkowns, public cMemCheck cTopoObs* getObs(size_t i) {return mObs.at(i);} std::vector & getAllObs() {return mObs;} bool addObs(eTopoObsType type, cBA_Topo * aBA_Topo, const std::vector &pts, const std::vector & vals, const cResidualWeighterExplicit & aWeights); - virtual void makeConstraints(cResolSysNonLinear & aSys) = 0; // add constraints for current set - virtual bool initialize() = 0; // initialize set parameters, after all obs and points were added + virtual void makeConstraints(cResolSysNonLinear & aSys) = 0; ///< add constraints for current set + virtual bool initialize() = 0; ///< initialize set parameters, after all obs and points were added + bool isInit() const {return mInit;} protected: cTopoObsSet(cBA_Topo *aBA_Topo, eTopoObsSetType type); cTopoObsSet(cTopoObsSet const&) = delete; @@ -42,9 +43,10 @@ class cTopoObsSet : public cObjWithUnkowns, public cMemCheck eTopoObsSetType mType; std::vector mObs; //cObjWithUnkowns * mUK; - std::vector mParams; //the only copy of the parameters - std::vector mAllowedObsTypes;//to check if new obs are allowed + std::vector mParams; ///< the only copy of the parameters + std::vector mAllowedObsTypes; ///< to check if new obs are allowed cBA_Topo * mBA_Topo; + bool mInit; ///< is the set initialized }; /** @@ -73,7 +75,7 @@ class cTopoObsSetStation : public cTopoObsSet void OnUpdate() override; ///< "reaction" after linear update, eventually update inversion virtual std::string toString() const override; void makeConstraints(cResolSysNonLinear &aSys) override; - virtual bool initialize() override; // initialize rotation + virtual bool initialize() override; ///< initialize rotation void setOrigin(std::string _OriginName); void PushRotObs(std::vector & aVObs) const; diff --git a/MMVII/src/Topo/ctopopoint.cpp b/MMVII/src/Topo/ctopopoint.cpp index c336ab266e..5c2b908546 100644 --- a/MMVII/src/Topo/ctopopoint.cpp +++ b/MMVII/src/Topo/ctopopoint.cpp @@ -46,7 +46,7 @@ void cTopoPoint::findUK(const std::vector & vGCP, cPhotogrammetricPro mPt = &gcp->mGCP_UK.at(i)->Pt(); //< use existing unknown if available mInitCoord = *mPt; #ifdef VERBOSE_TOPO - StdOut() << "is a GCP with existing unknowns: "<<*mPt<<" "< &_vertDefl); std::string toString(); const cPtxd & getInitCoord() const {return mInitCoord;} - std::string getName() {return mName;} + std::string getName() const {return mName;} std::vector getIndices(); + bool isInit() const {return mPt->IsValid();} protected: std::string mName; cPt3dr mInitCoord; //< coord initialized only for pure topo points, may be also reference coordinates if not free for any type of point diff --git a/MMVII/src/Topo/topoinit.cpp b/MMVII/src/Topo/topoinit.cpp new file mode 100644 index 0000000000..d1b1356205 --- /dev/null +++ b/MMVII/src/Topo/topoinit.cpp @@ -0,0 +1,146 @@ +#include "topoinit.h" + +#include "ctopopoint.h" +#include "ctopoobs.h" +#include "ctopoobsset.h" + + +namespace MMVII +{ + +bool tryInit3Obs1Station(cTopoPoint & aPtToInit, tStationsMap &stationsMap) // try 3 obs from one station +{ + for (auto& [aOriginPt, aStationVect] : stationsMap) + { + if (!aOriginPt->isInit()) + continue; + for (auto & aStation: aStationVect) + { + if (!aStation->isInit()) + continue; + cTopoObs * obs_az = nullptr; + cTopoObs * obs_zen = nullptr; + cTopoObs * obs_dist = nullptr; + cTopoObs * obs_dx = nullptr; + cTopoObs * obs_dy = nullptr; + cTopoObs * obs_dz = nullptr; + for (const auto & aObs: aStation->getAllObs()) + { + if (aObs->getPointName(1)==aPtToInit.getName()) + { + switch (aObs->getType()) { + case eTopoObsType::eHz: + obs_az = aObs; + break; + case eTopoObsType::eZen: + obs_zen = aObs; + break; + case eTopoObsType::eDist: + obs_dist = aObs; + break; + case eTopoObsType::eDX: + obs_dx = aObs; + break; + case eTopoObsType::eDY: + obs_dy = aObs; + break; + case eTopoObsType::eDZ: + obs_dz = aObs; + break; + default: + break; + } + } + } + if (obs_az && obs_zen && obs_dist) + { +#ifdef VERBOSE_TOPO + StdOut() << "Init as angles and distance from one station on " << aOriginPt->getName() << "\n"; +#endif + cPt3dr a3DVect; + double d0 = obs_dist->getMeasures()[0]*sin(obs_zen->getMeasures()[0]); + a3DVect.x() = d0*sin(obs_az->getMeasures()[0]); + a3DVect.y() = d0*cos(obs_az->getMeasures()[0]); + a3DVect.z() = obs_dist->getMeasures()[0]*cos(obs_zen->getMeasures()[0]); + *aPtToInit.getPt() = *aOriginPt->getPt() + + (aStation->getRotSysCo2Vert() * aStation->getRotVert2Instr()).Inverse(a3DVect); + return true; + } + if (obs_dx && obs_dy && obs_dz) + { +#ifdef VERBOSE_TOPO + StdOut() << "Init as dx dy dz from one station on " << aOriginPt->getName() << "\n"; +#endif + cPt3dr a3DVect( obs_dx->getMeasures()[0], + obs_dy->getMeasures()[0], + obs_dz->getMeasures()[0] + ); + *aPtToInit.getPt() = *aOriginPt->getPt() + + (aStation->getRotSysCo2Vert() * aStation->getRotVert2Instr()).Inverse(a3DVect); + return true; + } + } + } + return false; +} + + +bool tryInitVertStations(cTopoPoint & aPtToInit, tStationsMap &stationsMap) // try bearing and distance from verticalized stations +{ + for (auto& [aOriginPt, aStationVect] : stationsMap) + { + tREAL8 az=NAN, zen=NAN, dist=NAN; + cTopoObsSetStation * aStationHz = nullptr; + if (!aOriginPt->isInit()) + continue; + for (auto & aStation: aStationVect) + { + for (const auto & aObs: aStation->getAllObs()) + { + if (aObs->getPointName(1)==aPtToInit.getName()) + { + // special case for verticalized stations: each obs can be in different stations + if (!std::isfinite(az)) + if ((aObs->getType()==eTopoObsType::eHz) && (aStation->isInit()) + && ((aStation->getOriStatus()==eTopoStOriStat::eTopoStOriFixed) + || (aStation->getOriStatus()==eTopoStOriStat::eTopoStOriVert))) + { + az = aObs->getMeasures()[0]; + aStationHz = aStation; + } + if (!std::isfinite(zen)) + if ((aObs->getType()==eTopoObsType::eZen) && ( + (aStation->getOriStatus()==eTopoStOriStat::eTopoStOriFixed) + || (aStation->getOriStatus()==eTopoStOriStat::eTopoStOriVert)) ) + zen = aObs->getMeasures()[0]; + if (!std::isfinite(dist)) + if (aObs->getType()==eTopoObsType::eDist) + dist = aObs->getMeasures()[0]; + + } + } + } + if (aStationHz && std::isfinite(az) && std::isfinite(zen) && std::isfinite(dist)) + { +#ifdef VERBOSE_TOPO + StdOut() << "Init as bearing and distance from " << aOriginPt->getName() << "\n"; +#endif + cPt3dr a3DVect; + double d0 = dist*sin(zen); + a3DVect.x() = d0*sin(az); + a3DVect.y() = d0*cos(az); + a3DVect.z() = dist*cos(zen); + *aPtToInit.getPt() = *aOriginPt->getPt() + + (aStationHz->getRotSysCo2Vert() * aStationHz->getRotVert2Instr()).Inverse(a3DVect); + return true; + } + } + return false; +} + + +// try resection + + + +} diff --git a/MMVII/src/Topo/topoinit.h b/MMVII/src/Topo/topoinit.h new file mode 100644 index 0000000000..0669c623e1 --- /dev/null +++ b/MMVII/src/Topo/topoinit.h @@ -0,0 +1,17 @@ +#ifndef TOPOINIT_H +#define TOPOINIT_H + +#include "MMVII_Topo.h" + +namespace MMVII +{ + +/** \file topoinit.h + \brief 3D points initialization from topo measurements +*/ + +bool tryInit3Obs1Station(cTopoPoint & aPtToInit, tStationsMap &stationsMap); +bool tryInitVertStations(cTopoPoint & aPtToInit, tStationsMap &stationsMap); + +} +#endif // TOPOINIT_H