Skip to content

Commit

Permalink
topo: add G0
Browse files Browse the repository at this point in the history
  • Loading branch information
@jmmuller
jmmuller committed Apr 22, 2024
1 parent 3f09710 commit 89a8e95
Show file tree
Hide file tree
Showing 10 changed files with 175 additions and 57 deletions.
2 changes: 1 addition & 1 deletion MMVII/src/SymbDerGen/Formulas_Topo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ class cFormulaTopoHz
auto val = aVObs[9];
cPtxd<tUk,3> aP_to_instr = aPoseInstr.Inverse().Value(aP_to);

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

return { az - val };
}
Expand Down
27 changes: 18 additions & 9 deletions MMVII/src/Topo/Topo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,8 @@ void cBA_Topo::FromData(const cTopoData &aTopoData, const std::vector<cBA_GCP *>
case eTopoObsSetType::eStation:
{
cTopoObsSetStation * st1 = static_cast<cTopoObsSetStation*>(aSet);
st1->setIsOriented(aSetData.mStationIsOriented.value_or(false));
st1->setIsVericalized(aSetData.mStationIsVericalized.value_or(false));
std::string aOriginName;
MMVII_INTERNAL_ASSERT_User(st1->getAllObs().size()>0, eTyUEr::eUnClassedError, "Error: Obs Set without obs.")
aOriginName = aSet->getObs(0)->getPointName(0);
Expand All @@ -127,6 +129,9 @@ void cBA_Topo::FromData(const cTopoData &aTopoData, const std::vector<cBA_GCP *>
default:
MMVII_INTERNAL_ASSERT_User(false, eTyUEr::eUnClassedError, "Error: unknown eTopoObsSetType.")
}

MMVII_INTERNAL_ASSERT_User(aSet->initialize(), eTyUEr::eUnClassedError,
"Error: Station initialization failed.")
}
mIsReady = true;
}
Expand Down Expand Up @@ -269,15 +274,11 @@ void cBA_Topo::AddTopoEquations(cResolSysNonLinear<tREAL8> & aSys)

//-------------------------------------------------------------------


void BenchTopoComp(cParamExeBench & aParam)
void BenchTopoComp1example(cTopoData aTopoData, tREAL4 targetSigma0)
{
if (! aParam.NewBench("TopoComp")) return;

cSetInterUK_MultipeObj<double> aSetIntervMultObj;
double aLVM = 0.;

cTopoData aTopoData = cTopoData::createEx1();
aTopoData.ToFile(cMMVII_Appli::TmpDirTestMMVII()+"bench-in.json");

cBA_Topo aTopo(nullptr, cMMVII_Appli::TmpDirTestMMVII()+"bench-in.json");
Expand All @@ -291,7 +292,7 @@ void BenchTopoComp(cParamExeBench & aParam)
cDenseVect<double> aVUk = aSetIntervMultObj.GetVUnKnowns();
cResolSysNonLinear<double> aSys = cResolSysNonLinear<double>(eModeSSR::eSSR_LsqNormSparse,aVUk);

for (int iter=0; iter<5; ++iter)
for (int iter=0; iter<6; ++iter)
{
#ifdef VERBOSE_TOPO
std::cout<<"Iter "<<iter<<std::endl;
Expand All @@ -307,11 +308,19 @@ void BenchTopoComp(cParamExeBench & aParam)
}
aTopo.ToFile(cMMVII_Appli::TmpDirTestMMVII()+"bench-out.json");

auto targetSigma0 = 0.707107;
MMVII_INTERNAL_ASSERT_bench(std::abs(aTopo.Sigma0()-targetSigma0)<1e-6,"TopoComp sigma0 final");
MMVII_INTERNAL_ASSERT_bench(std::abs(aTopo.Sigma0()-targetSigma0)<1e-5,"TopoComp sigma0 final");

//std::cout<<"Bench Topo finished."<<std::endl;
aSetIntervMultObj.SIUK_Reset();
}

void BenchTopoComp(cParamExeBench & aParam)
{
if (! aParam.NewBench("TopoComp")) return;

BenchTopoComp1example(cTopoData::createEx1(), 0.70711);
BenchTopoComp1example(cTopoData::createEx3(), 1.41421);

//std::cout<<"Bench Topo finished."<<std::endl;
aParam.EndBench();
return;
}
Expand Down
2 changes: 1 addition & 1 deletion MMVII/src/Topo/Topo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ class cSensorCamPC;
class cTopoObsSet;
class cBA_GCP;

class cBA_Topo
class cBA_Topo : public cMemCheck
{
friend class cTopoData;
public :
Expand Down
73 changes: 59 additions & 14 deletions MMVII/src/Topo/ctopodata.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,29 @@ void AddData(const cAuxAr2007 & anAux, cTopoObsData & aTopoObs)


// -------------------------

void cTopoObsSetData::AddData(const cAuxAr2007 & anAuxInit)
{
cAuxAr2007 anAux("TopoObsSetData",anAuxInit);
//std::cout<<"Add data obs set '"<<toString()<<"'"<<std::endl;
MMVII::EnumAddData(anAux,mType,"Type");
MMVII::AddData(cAuxAr2007("AllObs",anAux),mObs);

AddOptData(anAux,"StationIsVericalized",mStationIsVericalized);
AddOptData(anAux,"StationIsOriented",mStationIsOriented);
AddOptData(anAux,"G0",mStationG0);
//AddOptData(anAux,"StationRot",mRotVert2Instr); // TODO
}


void AddData(const cAuxAr2007 & anAux, cTopoObsSetData &aObsSet)
{
aObsSet.AddData(anAux);
}


// ------------------------------------

cTopoData::cTopoData(cBA_Topo* aBA_topo)
{
for (auto & [aName, aPt] : aBA_topo->getAllPts())
Expand All @@ -78,6 +101,22 @@ cTopoData::cTopoData(cBA_Topo* aBA_topo)
{
cTopoObsSetData aSetData;
aSetData.mType = aSet->mType;
switch (aSet->mType) {
case eTopoObsSetType::eStation:
{
cTopoObsSetStation* set = dynamic_cast<cTopoObsSetStation*>(aSet);
if (!set)
MMVII_INTERNAL_ERROR("error set type")
aSetData.mStationIsOriented = set->isOriented();
aSetData.mStationIsVericalized = set->isVericalized();
aSetData.mStationG0 = set->getG0();
aSetData.mRotVert2Instr = set->getRotVert2Instr();
break;
}
default:
MMVII_INTERNAL_ERROR("unknown obs set type")
}

for (auto & aObs : aSet->mObs)
{
cTopoObsData aObsData = {
Expand Down Expand Up @@ -187,6 +226,12 @@ bool cTopoData::addObs(eCompObsTypes code, const std::string & nameFrom, const s
case eCompObsTypes::eCompDist:
aObsData = {eTopoObsType::eDist, {nameFrom,nameTo}, {val}, {sigma}};
break;
case eCompObsTypes::eCompHz:
aObsData = {eTopoObsType::eHz, {nameFrom,nameTo}, {val}, {sigma}};
break;
case eCompObsTypes::eCompZen:
aObsData = {eTopoObsType::eZen, {nameFrom,nameTo}, {val}, {sigma}};
break;
default:
StdOut() << "Error, unknown obs code " << (int)code <<".\n";
return false;
Expand Down Expand Up @@ -257,26 +302,26 @@ void cTopoData::createEx2()

cTopoData cTopoData::createEx3()
{
cTopoPointData aPt1 = {"St1",cPt3dr(100,100,100),false,cPt3dr(0.01,0.01,0.01)};
cTopoPointData aPt2 = {"Tr1",cPt3dr(105,115,105),true};
cTopoPointData aPt1 = {"Ori1",cPt3dr(100,110,100),false,cPt3dr(0.01,0.01,0.01)};
cTopoPointData aPt2 = {"St1",cPt3dr(100,100,100),false,cPt3dr(0.01,0.01,0.01)};
cTopoPointData aPt3 = {"Tr1",cPt3dr(105,115,105),true}; // 107.072, 107.072, 100

cTopoObsData aObs1 = {eTopoObsType::eHz, {"St1", "Tr1"}, {M_PI/2.}, {0.001}};
cTopoObsData aObs2 = {eTopoObsType::eZen, {"St1", "Tr1"}, {0.}, {0.001}};
cTopoObsData aObs3 = {eTopoObsType::eDist, {"St1", "Tr1"}, {10.}, {0.001}};
cTopoObsData aObs4 = {eTopoObsType::eDist, {"St1", "Tr1"}, {10.002}, {0.001}};
double g0 = 2.2;
cTopoObsData aObs1 = {eTopoObsType::eHz, {"St1", "Ori1"}, {0. - g0}, {0.001}};
cTopoObsData aObs2 = {eTopoObsType::eHz, {"St1", "Tr1"}, {M_PI/4. - g0}, {0.001}};
cTopoObsData aObs3 = {eTopoObsType::eZen, {"St1", "Tr1"}, {0.}, {0.001}};
cTopoObsData aObs4 = {eTopoObsType::eDist, {"St1", "Tr1"}, {10.}, {0.001}};
cTopoObsData aObs5 = {eTopoObsType::eDist, {"St1", "Tr1"}, {10.002}, {0.001}};

cTopoObsSetData aSet1;
aSet1.mType = eTopoObsSetType::eStation;
aSet1.mObs = {aObs1, aObs2, aObs3, aObs4};

cTopoObsData aObs5 = {eTopoObsType::eDist, {"Tr1", "St1"}, {10.002}, {0.001}};
cTopoObsSetData aSet2;
aSet2.mType = eTopoObsSetType::eStation;
aSet2.mObs = {aObs5};
aSet1.mStationIsVericalized = true;
aSet1.mStationIsOriented = false;
aSet1.mObs = {aObs1, aObs2, aObs3, aObs4, aObs5};

cTopoData aTopoData;
aTopoData.mAllPoints = {aPt1, aPt2};
aTopoData.mAllObsSets = {aSet1, aSet2};
aTopoData.mAllPoints = {aPt1, aPt2, aPt3};
aTopoData.mAllObsSets = {aSet1};

return aTopoData;
}
Expand Down
6 changes: 6 additions & 0 deletions MMVII/src/Topo/ctopodata.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,12 @@ class cTopoObsSetData
void AddData(const cAuxAr2007 & anAuxInit);
eTopoObsSetType mType;
std::vector<cTopoObsData> mObs;

// just for station
std::optional<bool> mStationIsVericalized;
std::optional<bool> mStationIsOriented;
std::optional<tREAL8> mStationG0; // just output
std::optional<cRotation3D<tREAL8>> mRotVert2Instr; // just output
};

void AddData(const cAuxAr2007 & anAux, cTopoObsSetData & aObsSet);
Expand Down
3 changes: 3 additions & 0 deletions MMVII/src/Topo/ctopoobs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,6 +170,9 @@ std::vector<tREAL8> cTopoObs::getVals() const
}*/
#ifdef VERBOSE_TOPO
std::cout<<vals.size()<<" values ";//<<std::endl;
for (auto&v: vals)
std::cout<<v<<" ";
std::cout<<"\n";
#endif
return vals;
}
Expand Down
2 changes: 1 addition & 1 deletion MMVII/src/Topo/ctopoobs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ template <class Type> class cResidualWeighterExplicit;
* @brief The cTopoObs class represents an observation between several points.
* It exists only in a cTopoObsSet because the obs may share parameters with other obs.
*/
class cTopoObs
class cTopoObs : public cMemCheck
{
friend class cTopoObsSet;
friend class cTopoData;
Expand Down
99 changes: 73 additions & 26 deletions MMVII/src/Topo/ctopoobsset.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,24 +10,6 @@
namespace MMVII
{

void cTopoObsSetData::AddData(const cAuxAr2007 & anAuxInit)
{
cAuxAr2007 anAux("TopoObsSetData",anAuxInit);
//std::cout<<"Add data obs set '"<<toString()<<"'"<<std::endl;
MMVII::EnumAddData(anAux,mType,"Type");
MMVII::AddData(cAuxAr2007("AllObs",anAux),mObs);
}


void AddData(const cAuxAr2007 & anAux, cTopoObsSetData &aObsSet)
{
aObsSet.AddData(anAux);
}


// ------------------------------------


cTopoObsSet::cTopoObsSet(cBA_Topo * aBA_Topo, eTopoObsSetType type):
mType(type), mBA_Topo(aBA_Topo)
{
Expand Down Expand Up @@ -105,8 +87,8 @@ std::vector<int> cTopoObsSet::getParamIndices() const

//----------------------------------------------------------------
cTopoObsSetStation::cTopoObsSetStation(cBA_Topo *aBA_Topo) :
cTopoObsSet(aBA_Topo, eTopoObsSetType::eStation), mIsVericalized(true), mIsOriented(true),
mRot(tRot::Identity()), mRotOmega({0.,0.,0.}), mOriginName(""),mPtOrigin(nullptr)
cTopoObsSet(aBA_Topo, eTopoObsSetType::eStation), mIsVericalized(true), mIsOriented(false),
mRotVert2Instr(tRot::Identity()), mRotOmega({0.,0.,0.}), mOriginName(""),mPtOrigin(nullptr)
{
}

Expand Down Expand Up @@ -137,6 +119,7 @@ void cTopoObsSetStation::AddToSys(cSetInterUK_MultipeObj<tREAL8> & aSet)
std::cout<<"AddToSys SetStation mRotOmega "<<&mRotOmega<<std::endl;
#endif
aSet.AddOneObj(&mRotOmega);
aSet.AddOneObj(this); // to have OnUpdate called on SetVUnKnowns
}

void cTopoObsSetStation::createParams()
Expand All @@ -146,12 +129,19 @@ void cTopoObsSetStation::createParams()

void cTopoObsSetStation::OnUpdate()
{
// TODO: copy pose update
// like cPoseWithUK::OnUpdate(), without -...
mRotVert2Instr = mRotVert2Instr * cRotation3D<tREAL8>::RotFromAxiator(mRotOmega.Pt());

//std::cout<<" OnUpdate mRotOmega: "<<mRotOmega.Pt()<<"\n";

// now this have modify rotation, the "delta" is void :
mRotOmega.Pt() = cPt3dr(0,0,0);
}

void cTopoObsSetStation::PushRotObs(std::vector<double> & aVObs) const
{
mRot.Mat().PushByCol(aVObs);
// TODO: push rotLTF2Vert * mRotVert2Instr
mRotVert2Instr.Mat().PushByCol(aVObs);
}

std::string cTopoObsSetStation::toString() const
Expand All @@ -160,6 +150,12 @@ std::string cTopoObsSetStation::toString() const
oss<<" origin: "<<mOriginName;
if (mPtOrigin)
oss<<" "<<*mPtOrigin->getPt();
oss<<"\n mRotOmega: "<<mRotOmega.Pt()<<" ";
oss<<"\n mRot:\n";
oss<<" "<<mRotVert2Instr.AxeI()<<"\n";
oss<<" "<<mRotVert2Instr.AxeJ()<<"\n";
oss<<" "<<mRotVert2Instr.AxeK()<<"\n";

return cTopoObsSet::toString() + oss.str();
}

Expand All @@ -169,7 +165,6 @@ void cTopoObsSetStation::makeConstraints(cResolSysNonLinear<tREAL8> & aSys)
// TODO: depends on bubbling etc.
if (mIsVericalized && mIsOriented)
{
mRot = tRot::Identity();
mRotOmega.Pt() = {0.,0.,0.};
#ifdef VERBOSE_TOPO
std::cout<<"Freeze rotation for "<<&mRotOmega<<std::endl;
Expand All @@ -178,11 +173,58 @@ void cTopoObsSetStation::makeConstraints(cResolSysNonLinear<tREAL8> & aSys)
aSys.SetFrozenVarCurVal(mRotOmega,mRotOmega.Pt());
//for (int i=mRotOmega.IndUk0()+3;i<mRotOmega.IndUk1();++i)
// aSys.AddEqFixCurVar(i,0.001);
} else {
MMVII_INTERNAL_ASSERT_strong(false,"Not fixed orientation station is forbidden");
}
else if (mIsVericalized)
{
mRotOmega.Pt() = {0.,0.,0.};
#ifdef VERBOSE_TOPO
std::cout<<"Freeze bascule for "<<&mRotOmega<<std::endl;
std::cout<<" rotation indices "<<mRotOmega.IndUk0()<<"-"<<mRotOmega.IndUk1()-2<<std::endl;
#endif
aSys.SetFrozenVarCurVal(mRotOmega,mRotOmega.Pt().PtRawData(), 2); // not z
//for (int i=mRotOmega.IndUk0()+3;i<mRotOmega.IndUk1()-1;++i)
// aSys.AddEqFixCurVar(i,0.001);
}
else
{
MMVII_INTERNAL_ASSERT_strong(false,"Not fixed orientation station is forbidden for now");
}
}


bool cTopoObsSetStation::initialize()
{
#ifdef VERBOSE_TOPO
std::cout<<"cTopoObsSetStation::initialize "<<mOriginName<<std::endl;
#endif
if (mIsVericalized && mIsOriented)
{
return true; // nothing to do
}
if (mIsVericalized) // compute initial G0
{
for (auto & obs: mObs)
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));
tREAL8 G0 = atan2( aPtTo.getPt()->x() - mPtOrigin->getPt()->x(),
aPtTo.getPt()->y() - mPtOrigin->getPt()->y())
- obs->getMeasures().at(0);
//std::cout<<"Init G0: "<<G0<<std::endl;
mRotVert2Instr = mRotVert2Instr * cRotation3D<tREAL8>::RotFromAxiator({0., 0., G0});
return true;
}
// if there is no Hz mes, fix ori (TODO: fail if Hz but targets not init)
mIsOriented = true;
return initialize();
}
MMVII_DEV_WARNING("cTopoObsSetStation initialization not ready for not vericalized stations.")
return false;
}


void cTopoObsSetStation::setOrigin(std::string _OriginName, bool _IsVericalized)
{
#ifdef VERBOSE_TOPO
Expand All @@ -191,11 +233,16 @@ void cTopoObsSetStation::setOrigin(std::string _OriginName, bool _IsVericalized)
mPtOrigin = &mBA_Topo->getPoint(_OriginName);
mOriginName = _OriginName;
mIsVericalized = _IsVericalized;
mRot = tRot::Identity();
mRotVert2Instr = tRot::Identity();
mRotOmega.Pt() = {0.,0.,0.};

}

tREAL8 cTopoObsSetStation::getG0() const
{
return atan2(mRotVert2Instr.Mat().GetElem(0,1), mRotVert2Instr.Mat().GetElem(0,0));
}

/*
//----------------------------------------------------------------
cTopoObsSetDistParam::cTopoObsSetDistParam(cTopoData& aTopoData) :
Expand Down
Loading

0 comments on commit 89a8e95

Please sign in to comment.