Skip to content

Commit

Permalink
topo: fix, doc & cleaning
Browse files Browse the repository at this point in the history
  • Loading branch information
@jmmuller
jmmuller committed Aug 23, 2024
1 parent c4ca534 commit e2cc0dc
Show file tree
Hide file tree
Showing 18 changed files with 224 additions and 87 deletions.
Binary file modified MMVII/Doc/CommandReferences/ImagesComRef/topo.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
18 changes: 9 additions & 9 deletions MMVII/Doc/CommandReferences/ImagesComRef/topo.svg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
2 changes: 1 addition & 1 deletion MMVII/Doc/CommandReferences/SysCo.tex
View file
Original file line number Diff line number Diff line change
Expand Up @@ -421,7 +421,7 @@ \subsection{Frames for survey stations}
\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{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}
Expand Down
111 changes: 94 additions & 17 deletions MMVII/Doc/Programmer/NonLinearOptim.tex
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1365,41 +1365,118 @@ \subsubsection{Integration into photogrammetric compensation}

\subsubsection{Topo survey equations}

For \texttt{cTopoObsSetStation}, all the measurements are expressed in the local survey station frame.
The adjustment unknowns are in RTL.

The input data are:
For \texttt{cTopoObsSetStation}, all the measurements are expressed in the instrument's local frame.

\begin{figure}[!h]
\centering
\includegraphics[width=8cm]{Programmer/framesTopo.png}
\caption{Frames types:
\texttt{Green}: projection SysCo.
\texttt{Blue}: RTL SysCo for the computation.
\texttt{Purple}: 3D point local vertical frame tangent to the ellipsoid.
\texttt{Yellow}: instrument frame.
}
\label{fig:topoFrames}
\end{figure}

Each instrument orientation rotation from RTL is computed via the local vertical frame at its origin:

\begin{equation}
R_{RTL \rightarrow Instr} = R_{Vert \rightarrow Instr} \cdot R_{RTL \rightarrow Vert}
\end{equation}


Where $R_{RTL \rightarrow Vert}$ is computed bu the SysCo from station origin position and
$R_{Vert \rightarrow Instr}$ is unknown, with a degree of liberty depending on the station orientation constraint.

It is recorded in \texttt{cTopoObsSetStation} as:

\begin{lstlisting}
tRot mRotSysCo2Vert; //< rotation between global SysCo and local vertical frame
tRot mRotVert2Instr; //< current value for rotation from local vertical frame to instrument frame
cPt3dr_UK mRotOmega; //< mRotVert2Instr unknown
\end{lstlisting}

The possible orientation constraints are:
\begin{itemize}
\item \texttt{\#FIX}: \texttt{mRotOmega} is fixed for $x$, $y$ and $z$
\item \texttt{\#VERT}: \texttt{mRotOmega} is fixed for $x$ and $y$
\item \texttt{\#BASC}: \texttt{mRotOmega} has no fixed component
\end{itemize}


The transformation from RTL to instrument local frame is:

\begin{equation}
T_{Instr} = R_{RTL \rightarrow Instr} \cdot (T_{RTL} - O_{RTL})
\end{equation}

Where:

\begin{itemize}
\item $T_{Instr}$: target point in instrument local frame
\item $O_{RTL}$: station origin point in RTL SysCo
\item $T_{RTL}$: target point in RTL SysCo
\item $R_{St, RTL}$: station origin point in station local frame
\item $\omega_R$: the rotation axiator unknown
\item $l$: the measurement value
\item $R_{RTL \rightarrow Instr}$: rotation from RTL to instrument frame
\end{itemize}

At first we compute target coordinates in station local frame $T_{loc}$:

$$ T_{loc} = R_{Station, RTL}^T . (T_{RTL} - O_{RTL}) $$

Then for each type of observation:
Then for each type of observation ($l$ being the measurement value):

\begin{itemize}
\item {\tt cFormulaTopoDX}: $$ residual = T_{loc,X} - l$$
\item {\tt cFormulaTopoDY}: $$ residual = T_{loc,X} - l$$
\item {\tt cFormulaTopoDZ}: $$ residual = T_{loc,X} - l$$
\item {\tt cFormulaTopoDX}: $$ residual = T_{Instr_X} - l$$
\item {\tt cFormulaTopoDY}: $$ residual = T_{Instr_Y} - l$$
\item {\tt cFormulaTopoDZ}: $$ residual = T_{Instr_Z} - l$$

\item {\tt cFormulaTopoHz}: $$ residual = \arctan\left(T_{loc,X}, T_{loc,Y}\right) - l $$
\item {\tt cFormulaTopoHz}: $$ residual = \arctan\left(T_{Instr_X}, T_{Instr_Y}\right) - l $$
\item {\tt cFormulaTopoZen}:
$$ ref = 0.12 . \frac { hz\_dist\_ellips\left( T, O \right) }
{ 2 . earth\_radius} $$
$$ d_{hz} = \| T_{loc,X}, T_{loc,Y} \| $$
$$ residual = \arctan\left(d_{hz}, T_{loc,Z}\right) - ref - l $$
\item {\tt cFormulaTopoDist}: $$ residual = \| T_{lol} \| - l $$
$$ d_{hz} = \| T_{Instr_X}, T_{Instr_Y} \| $$
$$ residual = \arctan\left(d_{hz}, T_{Instr_Z}\right) - ref - l $$
\item {\tt cFormulaTopoDist}: $$ residual = \| T_{Instr} \| - l $$

\end{itemize}

Angles residuals are put in $ [ -\pi, + \pi ] $ interval.
Angles residuals are in $\left[ -\pi, + \pi \right]$ interval.

This is implemented like this:
\begin{lstlisting}
class cFormulaTopoHz
{
public :
std::string FormulaName() const { return "TopoHz";}
std::vector<std::string> VNamesUnknowns() const
{
// Instrument pose with 6 unknowns : 3 for center, 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> aPoseInstr2RTL(aVUk,0,aVObs,0,true);
cPtxd<tUk,3> aP_to = VtoP3(aVUk,6);
auto val = aVObs[9];
cPtxd<tUk,3> aP_to_instr = aPoseInstr2RTL.Inverse().Value(aP_to);
auto az = ATan2( aP_to_instr.x(), aP_to_instr.y() );
return { DiffAngMod(az, val) };
}
};

\end{lstlisting}

%---------------------------------------------
%---------------------------------------------
Expand Down
2 changes: 1 addition & 1 deletion MMVII/include/MMVII_SysCo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ public :
virtual tPt Value(const tPt &) const override = 0; //< to GeoC
virtual tPt Inverse(const tPt &) const override = 0; //< from GeoC

virtual cRotation3D<tREAL8> getVertical(const tPt &) const; //< get rotation from SysCo origin to vertical at point
virtual cRotation3D<tREAL8> getRot2Vertical(const tPt &) const; //< get rotation from SysCo origin to vertical at point
virtual tREAL8 getRadiusApprox(const tPt &in) const; //< approximate earth total curvature radius at a point
virtual tREAL8 getDistHzApprox(const tPt & aPtA, const tPt & aPtB) const; //< approximate horizontal distance (along ellipsoid) from one point to an other

Expand Down
2 changes: 1 addition & 1 deletion MMVII/include/MMVII_enums.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -239,7 +239,7 @@ enum class eTyUEr
eNoNumberPixel,
eNoCameraName,
eMultipleTargetInOneImage,
eBadSysCo,
eSysCo,
eConstraintsError,
eUnClassedError,
eNbVals
Expand Down
1 change: 1 addition & 0 deletions MMVII/include/MMVII_nums.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ double Cst_KthVal(const std::vector<double> &, double aProportion);
double Average(const std::vector<double> &);

tREAL8 AngleInRad(eTyUnitAngle);
bool AssertRadAngleInOneRound(tREAL8 aAngleRad, bool makeError=true);

// some time needs a null val for any type with + (neutral for +)

Expand Down
2 changes: 1 addition & 1 deletion MMVII/src/Serial/ElemStrToVal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -345,7 +345,7 @@ template<> cE2Str<eTyUEr>::tMapE2Str cE2Str<eTyUEr>::mE2S
{eTyUEr::eNoNumberPixel,"NoNumberPixel"},
{eTyUEr::eNoCameraName,"NoCameraName"},
{eTyUEr::eMultipleTargetInOneImage,"MultipleTargetInOneImage,"},
{eTyUEr::eBadSysCo,"BadSysCo"},
{eTyUEr::eSysCo,"SysCo"},
{eTyUEr::eConstraintsError,"ConstraintsError"},
{eTyUEr::eUnClassedError,"UnClassedError"}
};
Expand Down
36 changes: 18 additions & 18 deletions MMVII/src/SymbDerGen/Formulas_Topo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ class cFormulaTopoHz

std::vector<std::string> VNamesUnknowns() const
{
// Instrument pose with 6 unknowns : 3 for centers, 3 for axiator
// Instrument pose with 6 unknowns : 3 for center, 3 for axiator
// target pose with 3 unknowns : 3 for center
return Append(NamesPose("Ci","Wi"),NamesP3("P_to"));
}
Expand All @@ -52,10 +52,10 @@ class cFormulaTopoHz
const std::vector<tUk> & aVObs
) const
{
cPoseF<tUk> aPoseInstr(aVUk,0,aVObs,0,true);
cPoseF<tUk> aPoseInstr2RTL(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);
cPtxd<tUk,3> aP_to_instr = aPoseInstr2RTL.Inverse().Value(aP_to);

auto az = ATan2( aP_to_instr.x(), aP_to_instr.y() );

Expand All @@ -72,7 +72,7 @@ class cFormulaTopoZen

std::vector<std::string> VNamesUnknowns() const
{
// Instrument pose with 6 unknowns : 3 for centers, 3 for axiator
// Instrument pose with 6 unknowns : 3 for center, 3 for axiator
// target pose with 3 unknowns : 3 for center
return Append(NamesPose("Ci","Wi"),NamesP3("P_to"));
}
Expand All @@ -92,11 +92,11 @@ class cFormulaTopoZen
const std::vector<tUk> & aVObs
) const
{
cPoseF<tUk> aPoseInstr(aVUk,0,aVObs,0,true);
cPoseF<tUk> aPoseInstr2RTL(aVUk,0,aVObs,0,true);
cPtxd<tUk,3> aP_to = VtoP3(aVUk,6);
auto ref_cor = aVObs[9];
auto val = aVObs[10];
cPtxd<tUk,3> aP_to_instr = aPoseInstr.Inverse().Value(aP_to);
cPtxd<tUk,3> aP_to_instr = aPoseInstr2RTL.Inverse().Value(aP_to);

auto dist_hz = Norm2( cPtxd<tUk,2>(aP_to_instr.x(), aP_to_instr.y() ) );

Expand All @@ -115,7 +115,7 @@ class cFormulaTopoDist

std::vector<std::string> VNamesUnknowns() const
{
// Instrument pose with 6 unknowns : 3 for centers, 3 for axiator
// Instrument pose with 6 unknowns : 3 for center, 3 for axiator
// target pose with 3 unknowns : 3 for center
return Append(NamesPose("Ci","Wi"),NamesP3("P_to"));
}
Expand All @@ -134,10 +134,10 @@ class cFormulaTopoDist
const std::vector<tUk> & aVObs
) const
{
cPoseF<tUk> aPoseInstr(aVUk,0,aVObs,0,true);
cPoseF<tUk> aPoseInstr2RTL(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);
cPtxd<tUk,3> aP_to_instr = aPoseInstr2RTL.Inverse().Value(aP_to);

auto dist = Norm2(aP_to_instr);
return { dist - val };
Expand All @@ -154,7 +154,7 @@ class cFormulaTopoDX

std::vector<std::string> VNamesUnknowns() const
{
// Instrument pose with 6 unknowns : 3 for centers, 3 for axiator
// Instrument pose with 6 unknowns : 3 for center, 3 for axiator
// target pose with 3 unknowns : 3 for center
return Append(NamesPose("Ci","Wi"),NamesP3("P_to"));
}
Expand All @@ -173,10 +173,10 @@ class cFormulaTopoDX
const std::vector<tUk> & aVObs
) const
{
cPoseF<tUk> aPoseInstr(aVUk,0,aVObs,0,true);
cPoseF<tUk> aPoseInstr2RTL(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);
cPtxd<tUk,3> aP_to_instr = aPoseInstr2RTL.Inverse().Value(aP_to);

return { aP_to_instr.x() - val };
}
Expand All @@ -191,7 +191,7 @@ class cFormulaTopoDY

std::vector<std::string> VNamesUnknowns() const
{
// Instrument pose with 6 unknowns : 3 for centers, 3 for axiator
// Instrument pose with 6 unknowns : 3 for center, 3 for axiator
// target pose with 3 unknowns : 3 for center
return Append(NamesPose("Ci","Wi"),NamesP3("P_to"));
}
Expand All @@ -210,10 +210,10 @@ class cFormulaTopoDY
const std::vector<tUk> & aVObs
) const
{
cPoseF<tUk> aPoseInstr(aVUk,0,aVObs,0,true);
cPoseF<tUk> aPoseInstr2RTL(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);
cPtxd<tUk,3> aP_to_instr = aPoseInstr2RTL.Inverse().Value(aP_to);

return { aP_to_instr.y() - val };
}
Expand All @@ -228,7 +228,7 @@ class cFormulaTopoDZ

std::vector<std::string> VNamesUnknowns() const
{
// Instrument pose with 6 unknowns : 3 for centers, 3 for axiator
// Instrument pose with 6 unknowns : 3 for center, 3 for axiator
// target pose with 3 unknowns : 3 for center
return Append(NamesPose("Ci","Wi"),NamesP3("P_to"));
}
Expand All @@ -247,10 +247,10 @@ class cFormulaTopoDZ
const std::vector<tUk> & aVObs
) const
{
cPoseF<tUk> aPoseInstr(aVUk,0,aVObs,0,true);
cPoseF<tUk> aPoseInstr2RTL(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);
cPtxd<tUk,3> aP_to_instr = aPoseInstr2RTL.Inverse().Value(aP_to);

return { aP_to_instr.z() - val };
}
Expand Down
Loading

0 comments on commit e2cc0dc

Please sign in to comment.