Skip to content

Commit 3a34182

Browse files
author
deseilligny
committed
Bckup, In NewClino, Init Done
1 parent ce13b06 commit 3a34182

File tree

12 files changed

+402
-58
lines changed

12 files changed

+402
-58
lines changed

‎MMVII/include/MMVII_InstrumentalBlock.h‎

Lines changed: 34 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -173,11 +173,17 @@ class cIrbCal_Clino1 : public cMemCheck
173173
cIrbCal_Clino1(const std::string & aName); //< "Real" constructor
174174
const std::string & Name() const; //< accessor
175175
void AddData(const cAuxAr2007 & anAux); //< serializer
176+
177+
void SetPNorm(const cPt3dr & aTr);
178+
179+
cP3dNormWithUK& CurPNorm();
180+
bool IsInit() const;
181+
void UnInit();
176182
private :
177-
std::string mName; //< name of the clino
178-
bool mIsInit; //< was values computed ?
179-
tRotR mOrientInBloc; //< Position in the block
180-
tREAL8 mSigmaR; //< sigma on orientation
183+
std::string mName; //< name of the clino
184+
bool mIsInit; //< was values computed ?
185+
cP3dNormWithUK * mTrInBlock; //< Position in the block
186+
tREAL8 mSigmaR; //< sigma a priori on orientation
181187
};
182188
void AddData(const cAuxAr2007 & anAux,cIrbCal_Clino1 & aClino);
183189

@@ -192,6 +198,9 @@ class cIrbCal_ClinoSet : public cMemCheck
192198
void AddData(const cAuxAr2007 & anAux);
193199
std::vector<std::string> VNames() const;
194200
size_t NbClino() const;
201+
202+
int IndexClinoFromName(const std::string& aName) const;
203+
195204
private :
196205
cIrbCal_Clino1 * ClinoFromName(const std::string& aName);
197206
void AddClino(const std::string &,bool SVP=false);
@@ -218,6 +227,17 @@ class cIrb_CstrRelRot
218227

219228
void AddData(const cAuxAr2007 & anAux,cIrb_CstrRelRot & aSigma);
220229

230+
class cIrb_CstrOrthog
231+
{
232+
public :
233+
cIrb_CstrOrthog(const tREAL8 & aSigma);
234+
cIrb_CstrOrthog();
235+
void AddData(const cAuxAr2007 & anAux);
236+
private :
237+
tREAL8 mSigma;
238+
};
239+
void AddData(const cAuxAr2007 & anAux,cIrb_CstrOrthog & aSigma);
240+
221241

222242
/*
223243
class cIrb_ConstrInstr
@@ -308,7 +328,11 @@ class cIrbCal_Block : public cMemCheck
308328
void AvgIndivSigma(); //< Set all sigma of object ir global average
309329
void AvgSigma();
310330
cIrb_Desc1Intsr & AddSigma_Indiv(std::string aN1,eTyInstr aType1);
331+
311332
void AddCstrRelRot(std::string aN1,std::string aN2,tREAL8 aSigma,tRotR aRot);
333+
void AddCstrRelOrthog(std::string aN1,std::string aN2,tREAL8 aSigma);
334+
335+
const std::map<tNamePair,cIrb_CstrOrthog> & CstrOrthog() const;
312336

313337
private :
314338

@@ -324,9 +348,14 @@ class cIrbCal_Block : public cMemCheck
324348
cIrbCal_CamSet mSetCams; //< Cameras used in the bloc
325349
cIrbCal_ClinoSet mSetClinos; //< Clinos used in the bloc
326350

351+
// A priori external constraint
327352
std::map<tNamePair,cIrb_SigmaInstr> mSigmaPair; //< Sigmas between pair of instr
328353
std::map<std::string,cIrb_Desc1Intsr> mDescrIndiv; //< Sigmas of each instrument
354+
355+
// A priori external constraint
329356
std::map<tNamePair,cIrb_CstrRelRot> mCstrRelRot;
357+
std::map<tNamePair,cIrb_CstrOrthog> mCstrOrthog;
358+
330359

331360

332361
};
@@ -422,7 +451,7 @@ class cIrbComp_TimeS : public cMemCheck
422451
// cIrbComp_Block & CompBlock() ; //< Accessor
423452

424453
// if not SVP and cannot compute : error
425-
void ComputePoseInstrument(bool SVP = false);
454+
void ComputePoseInstrument(const std::vector<int> & aVNumCam,bool SVP = false);
426455
void SetClinoValues(const cOneMesureClino&);
427456

428457
tREAL8 ScoreDirClino(const cPt3dr& aDir,size_t aKClino) const;

‎MMVII/include/MMVII_PCSens.h‎

Lines changed: 27 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -450,7 +450,7 @@ class cPoseWithUK : public cObjWithUnkowns<tREAL8>
450450
*/
451451
void PushObs(std::vector<double> &,bool TransposeMatr);
452452

453-
cPoseWithUK(const tPoseR & aPose);
453+
cPoseWithUK(const tPoseR & aPose);
454454
void SetPose(const tPoseR & aPose);
455455

456456
// different accessor to the pose
@@ -467,24 +467,47 @@ class cPoseWithUK : public cObjWithUnkowns<tREAL8>
467467
tPoseR & Pose() ;
468468

469469
/// The interval is specified as the object can be used as helperto to other classes
470-
void PutUknowsInSetInterval(cSetInterUK_MultipeObj<tREAL8> * aSetInterv) ;
470+
void PutUknowsInSpecifiedSetInterval(cSetInterUK_MultipeObj<tREAL8> * aSetInterv) ;
471471
void OnUpdate() override; // "reaction" after linear update
472-
void FillGetAdrInfoParam(cGetAdrInfoParam<tREAL8> &) override;
472+
void FillGetAdrInfoParam(cGetAdrInfoParam<tREAL8> &) override;
473473
// std::vector<tPtrOUK> GetAllUK() override;
474474

475475
// Val axiator should have to "equal" fix rot
476476
cPt3dr ValAxiatorFixRot(const cRotation3D<tREAL8> & aRotFix) const;
477477

478478

479479
private :
480-
cPoseWithUK(const cPoseWithUK&) = delete;
480+
cPoseWithUK(const cPoseWithUK&) = delete;
481481
void PutUknowsInSetInterval() override ; // add the interval on udpate
482482

483483
tPoseR mPose; ///< transformation Cam to Word
484484
cPt3dr mOmega; ///< vector for tiny rotation when used in unknown, mW in code gene ...
485485
};
486486
void AddData(const cAuxAr2007 & anAux,cPoseWithUK & aPUK);
487487

488+
/// class for modelizing a normaliezd 3D-Vect when used as unknown in non linear system
489+
class cP3dNormWithUK : public cObjWithUnkowns<tREAL8>
490+
{
491+
public :
492+
cP3dNormWithUK(const cPt3dr &aPt,const std::string& aNameType,const std::string & aNameGrp);
493+
~cP3dNormWithUK();
494+
void OnUpdate() override; // "reaction" after linear update
495+
void FillGetAdrInfoParam(cGetAdrInfoParam<tREAL8> &) override;
496+
497+
void SetPNorm(const cPt3dr & aTr);
498+
cPt3dr GetPNorm () const;
499+
private :
500+
// normalize , compute U & V, set mDuDv to 0
501+
void Init();
502+
void PutUknowsInSetInterval() override ;
503+
504+
cPt3dr mPNorm; // curent value of PNorm
505+
cPt3dr mU; // first vector Orthog to mPNorm
506+
cPt3dr mV; // seconf vector Orthog to PNorm
507+
cPt2dr mDuDv; //
508+
509+
};
510+
488511

489512
/** Class for modelizing the geometry of perspective-central image, contain essentially a pose (Centre+rotation)
490513
* and a pointer to a (generally) shared internall calibration

‎MMVII/include/MMVII_Ptxd.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -569,6 +569,8 @@ template <class Type> inline bool InfEq (const cPtxd<Type,4> & aP1,const cPtxd<
569569

570570

571571
template<class T,const int Dim> cPtxd<T,Dim> VUnit(const cPtxd<T,Dim> & aP);
572+
template <class TypePt> std::pair<TypePt,TypePt> OrthogonalizePair(const TypePt & aP1,const TypePt & aP2);
573+
572574
// template<const int Dim> cPtxd<tREAL8 ,Dim> VUnit(const cPtxd<tREAL8 ,Dim> & aP);
573575
// template<const int Dim> cPtxd<tREAL16,Dim> VUnit(const cPtxd<tREAL16,Dim> & aP);
574576

‎MMVII/include/MMVII_util_tpl.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,8 @@
66
#include "MMVII_enums.h"
77
#include "MMVII_Error.h"
88
#include "MMVII_nums.h"
9+
#include "MMVII_Stringifier.h"
10+
911

1012
namespace MMVII
1113
{
@@ -268,7 +270,7 @@ template<class TCont,class TKey> const typename TCont::mapped_type * MapGet(co
268270
auto anIter = aCont.find(aKey);
269271
if (anIter==aCont.end())
270272
{
271-
MMVII_INTERNAL_ASSERT_tiny(SVP,"MapGet");
273+
MMVII_INTERNAL_ASSERT_tiny(SVP,"MapGet for Key="+ cStrIO<TKey>::ToStr(aKey));
272274
return nullptr;
273275
}
274276
return & anIter->second;

‎MMVII/src/Bench/BenchGeom.cpp‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,41 @@
66
namespace MMVII
77
{
88

9+
void Bench_OrthogonalizePair(const cPt3dr& aP1, const cPt3dr & aP2)
10+
{
11+
auto [aQ1,aQ2] = OrthogonalizePair(aP1,aP2);
12+
13+
tREAL8 aDifN = Norm2(VUnit(aP1^aP2) - VUnit(aQ1^aQ2));
14+
tREAL8 aScal = Scal(aQ1,aQ2);
15+
tREAL8 aD1 = Norm2(VUnit(aP1)-aQ1);
16+
tREAL8 aD2 = Norm2(VUnit(aP2)-aQ2);
17+
18+
tREAL8 aD12 = Norm2(VUnit(aP1)-aQ2);
19+
tREAL8 aD21 = Norm2(VUnit(aP2)-aQ1);
20+
21+
MMVII_INTERNAL_ASSERT_bench(aDifN<1e-10,"Bench_OrthogonalizePair Normal");
22+
MMVII_INTERNAL_ASSERT_bench(aScal<1e-10,"Bench_OrthogonalizePair Scal");
23+
MMVII_INTERNAL_ASSERT_bench(std::abs(aD1-aD2)<1e-10,"Diff distl");
24+
MMVII_INTERNAL_ASSERT_bench(std::max(aD1,aD2)<std::min(aD12,aD21)+1e-10,"Order distl");
25+
26+
//StdOut() << " DifN=" << aDifN << " S=" << aScal << " " << aD1 << " " << aD2 << " " << aD12 << " " << aD21 << "\n";
27+
}
28+
29+
void Bench_OrthogonalizePair()
30+
{
31+
for (int aK=0 ; aK<100 ; aK++)
32+
{
33+
cPt3dr aP1 = cPt3dr::PRand();
34+
while (Norm2(aP1)<1e-1)
35+
aP1 = cPt3dr::PRand();
36+
cPt3dr aP2 = cPt3dr::PRand();
37+
while ((Norm2(aP2)<1e-1) || (AbsAngleTrnk(aP1,aP2)<1e-2))
38+
aP2 = cPt3dr::PRand();
39+
Bench_OrthogonalizePair(aP1,aP2);
40+
}
41+
42+
}
43+
944
template<class Type> void TplBenchRotation3D(cParamExeBench & aParam)
1045
{
1146

@@ -734,6 +769,7 @@ void BenchGeom(cParamExeBench & aParam)
734769
{
735770
if (! aParam.NewBench("Geom")) return;
736771

772+
Bench_OrthogonalizePair();
737773
BenchSeg2D();
738774

739775
BenchSampleQuat();

‎MMVII/src/Geoms/PtsBox.cpp‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -99,6 +99,24 @@ template class cComputeCentroids<std::vector<cPtxd<tREAL8,3> >>;
9999
template class cComputeCentroids<std::vector<cPtxd<tREAL16,3> >>;
100100

101101

102+
template <class TypePt> std::pair<TypePt,TypePt> OrthogonalizePair(const TypePt & aP1,const TypePt & aP2)
103+
{
104+
TypePt aN1 = VUnit(aP1);
105+
TypePt aN2 = VUnit(aP2);
106+
107+
TypePt a1P2 = VUnit(aN1+aN2);
108+
TypePt a1M2 = VUnit(aN1-aN2);
109+
110+
return std::pair<TypePt,TypePt>
111+
(
112+
VUnit(a1P2+a1M2),
113+
VUnit(a1P2-a1M2)
114+
);
115+
}
116+
#define MACRO_ORTHOG_PAIR(TYPE_PT)\
117+
template std::pair<TYPE_PT,TYPE_PT> OrthogonalizePair(const TYPE_PT & aP1,const TYPE_PT & aP2);
118+
119+
MACRO_ORTHOG_PAIR(cPt3dr)
102120

103121
/* ========================== */
104122
/* cSegment */

0 commit comments

Comments
 (0)