Skip to content

Commit 3cb355f

Browse files
author
deseilligny
committed
In clino formula
1 parent d81027e commit 3cb355f

File tree

13 files changed

+499
-193
lines changed

13 files changed

+499
-193
lines changed

‎MMVII/include/MMVII_InstrumentalBlock.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -97,7 +97,7 @@ class cIrbCal_Cam1 : public cMemCheck
9797
/** modify the pose, separate from constructor because must be done in calib init, after block creation */
9898
void SetPose(const tPoseR & aPose);
9999

100-
const tPoseR & PoseInBlock() const; //< Accessor
100+
tPoseR PoseInBlock() const; //< Accessor
101101
tPoseR PosBInSysA(const cIrbCal_Cam1 & aCamB) const;
102102

103103
cPoseWithUK& PoseUKInBlock();

‎MMVII/include/MMVII_PCSens.h‎

Lines changed: 172 additions & 128 deletions
Large diffs are not rendered by default.

‎MMVII/include/MMVII_PhgrDist.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,9 @@ std::vector<std::string> NamesP3(const std::string& aPref) ; /// x y z
9797
std::vector<std::string> NamesP2(const std::string& aPref) ; /// x y z
9898
std::vector<std::string> NamesMatr(const std::string& aPref,const cPt2di & aSz); /// m00 m10 m20 m01 ...
9999

100+
// aPref+aK0 .... aPref+aK0-1
101+
std::vector<std::string> VectNames(const std::string& aPref,int aK0,int aK1) ; /// x y z
102+
100103
/// Vector of name for a pose NameC-> center of proj, NameOmega-> vector rotation
101104
std::vector<std::string> NamesPose(const std::string& NameC ,const std::string& NameOmega);
102105

‎MMVII/src/Bench/BenchGeom.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -311,7 +311,8 @@ void BenchRotation3DReal8()
311311
for (int aKIter=0 ; aKIter<5; aKIter++)
312312
{
313313
cPt3dr W = aPUK.ValAxiatorFixRot(aRTarget);
314-
aPUK.Omega() = W;
314+
//aPUK.Omega() = W;
315+
aPUK.SetOmega(W);
315316
aPUK.OnUpdate();
316317
}
317318
tREAL8 aD = aRTarget.Mat().L2Dist(aPUK.Pose().Rot().Mat()) ;

‎MMVII/src/BundleAdjustment/Adjust_InstrumentalBlock.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -327,8 +327,8 @@ void cBA_BlockInstr::OneItere()
327327
void cBA_BlockInstr::AddGauge(bool InEq)
328328
{
329329
cPoseWithUK & aPBl = mMasterCam.PoseUKInBlock();
330-
cPt3dr & aC = aPBl.Center();
331-
cPt3dr & aW = aPBl.Omega();
330+
cPt3dr & aC = aPBl.GetRefTr();
331+
cPt3dr & aW = aPBl.GetRefOmega();
332332

333333
if (InEq)
334334
{

‎MMVII/src/BundleAdjustment/Bundle_BlocRig.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,7 @@ void cBA_BlocRig::SetFrozenVar(cResolSysNonLinear<tREAL8> & aSys)
113113
// "knows" the association between its member and local integers, that's why
114114
// we pass object and members to do the job
115115

116-
aSys.SetFrozenVarCurVal(aMPose,aMPose.Center());
116+
aSys.SetFrozenVarCurVal(aMPose,aMPose.Tr());
117117
aSys.SetFrozenVarCurVal(aMPose,aMPose.Omega());
118118
//
119119
// ... freeze the center
@@ -161,7 +161,7 @@ cPt3dr cBA_BlocRig::OnePairAddRigidityEquation(size_t aKS,size_t aKBl1,size_t aK
161161
bool ForJeanMimi = false;
162162
if (ForJeanMimi)
163163
{
164-
aPBl2.PushIndexes(aVInd,aPBl2.Center());
164+
aPBl2.PushIndexes(aVInd,aPBl2.Tr());
165165
aPBl2.PushIndexes(aVInd,aPBl2.Omega());
166166
}
167167
else

‎MMVII/src/Geom3D/GeomsMappings3D.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -309,6 +309,7 @@ template <class Type> cRotation3D<Type> cRotation3D<Type>::CompleteRON(const tP
309309
template <class Type> cRotation3D<Type> cRotation3D<Type>::RotFromAxiator(const tPt & anAxe)
310310
{
311311
Type aNorm = Norm2(anAxe);
312+
// if too small, normalisation would be unstable and approximation by MatProdVect will be enough accurate
312313
if (aNorm<1e-5)
313314
{
314315
cDenseMatrix<Type> aMW = cDenseMatrix<Type>::Identity(3) + MatProdVect(anAxe);

‎MMVII/src/Instrumental/cBlockCamInit.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,7 @@ void cSetSensSameId::Resize(size_t aSize)
5454
mVCams.resize(aSize);
5555
}
5656

57+
5758
const std::vector<cSensorCamPC*>& cSetSensSameId::VCams() const {return mVCams;}
5859

5960
const std::string & cSetSensSameId::Id() const {return mId;}

‎MMVII/src/Instrumental/cIrb_Cams.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@ cIrbComp_Cam1::cIrbComp_Cam1() :
2121
{
2222
}
2323

24+
2425
void cIrbComp_Cam1::Init(cSensorCamPC * aCamPC)
2526
{
2627
if (mCamPC)
@@ -129,7 +130,7 @@ void cIrbCal_Cam1::SetPose(const tPoseR & aPose)
129130
}
130131

131132
bool cIrbCal_Cam1::IsInit() const{ return mIsInit;}
132-
const tPoseR & cIrbCal_Cam1::PoseInBlock() const
133+
tPoseR cIrbCal_Cam1::PoseInBlock() const
133134
{
134135
MMVII_INTERNAL_ASSERT_tiny(mIsInit,"IrbCal_Cam1::PoseInBlock");
135136
return mPoseInBlock->Pose();

‎MMVII/src/Sensors/cSensorCamPC.cpp‎

Lines changed: 149 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -71,47 +71,151 @@ void cP3dNormWithUK::SetPNorm(const cPt3dr & aTr)
7171
Init();
7272
}
7373

74+
/* ******************************************************* */
75+
/* */
76+
/* cRotWithUK */
77+
/* */
78+
/* ******************************************************* */
79+
// cRotWithUK(const tRotR & aPose);
80+
81+
82+
cRotWithUK::cRotWithUK(const tRotR & aRot) :
83+
mRot (aRot),
84+
mOmega (0.0,0.0,0.0)
85+
{
86+
87+
}
88+
89+
// cRotWithUK();
90+
91+
cRotWithUK::cRotWithUK() :
92+
cRotWithUK (tRotR())
93+
{
94+
}
95+
96+
97+
cRotWithUK::~cRotWithUK()
98+
{
99+
OUK_Reset();
100+
}
101+
102+
const tRotR & cRotWithUK::Rot() const {return mRot;}
103+
void cRotWithUK::SetRot( const tRotR & aRot)
104+
{
105+
mOmega = cPt3dr(0.0,0.0,0.0);
106+
mRot= aRot;
107+
}
108+
109+
cPt3dr cRotWithUK::AxeI() const {return mRot.AxeI();}
110+
cPt3dr cRotWithUK::AxeJ() const {return mRot.AxeJ();}
111+
cPt3dr cRotWithUK::AxeK() const {return mRot.AxeK();}
112+
113+
114+
const cPt3dr & cRotWithUK::Omega() const {return mOmega;}
115+
cPt3dr & cRotWithUK::GetRefOmega() {return mOmega;}
116+
void cRotWithUK::SetOmega(const cPt3dr & anOmega) {mOmega = anOmega;}
117+
118+
void cRotWithUK::PushObs(std::vector<double> & aVObs,bool TransposeMatr)
119+
{
120+
if (TransposeMatr)
121+
mRot.Mat().PushByCol(aVObs);
122+
else
123+
mRot.Mat().PushByLine(aVObs);
124+
}
125+
126+
127+
void cRotWithUK::OnUpdate()
128+
{
129+
// used above formula to modify rotation
130+
mRot = mRot * cRotation3D<tREAL8>::RotFromAxiator(-mOmega);
131+
// now this have modify rotation, the "delta" is void :
132+
mOmega = cPt3dr(0,0,0);
133+
}
134+
135+
void cRotWithUK::FillGetAdrInfoParam(cGetAdrInfoParam<tREAL8> & aGAIP)
136+
{
137+
aGAIP.TestParam(this, &( mOmega.x()) ,"Wx");
138+
aGAIP.TestParam(this, &( mOmega.y()) ,"Wy");
139+
aGAIP.TestParam(this, &( mOmega.z()) ,"Wz");
140+
141+
SetNameTypeId(aGAIP);
142+
}
143+
144+
void cRotWithUK::PutUknowsInSpecifiedSetInterval(cSetInterUK_MultipeObj<tREAL8> * aSetInterv)
145+
{
146+
aSetInterv->AddOneInterv(mOmega);
147+
148+
}
149+
void cRotWithUK::PutUknowsInSetInterval()
150+
{
151+
PutUknowsInSpecifiedSetInterval(mSetInterv);
152+
}
153+
154+
155+
cPt3dr cRotWithUK::ValAxiatorFixRot(const tRotR & aRotFix) const
156+
{
157+
cDenseMatrix<tREAL8> aM = mRot.Mat().Transpose() * aRotFix.Mat();
158+
tREAL8 aZ = ( aM(1,0) - aM(0,1)) / 2.0;
159+
tREAL8 aY = (-aM(2,0) + aM(0,2)) / 2.0;
160+
tREAL8 aX = ( aM(2,1) - aM(1,2)) / 2.0;
161+
162+
return cPt3dr(aX,aY,aZ);
163+
}
164+
74165
/* ******************************************************* */
75166
/* */
76167
/* cPoseWithUK */
77168
/* */
78169
/* ******************************************************* */
79170

80171
cPoseWithUK::cPoseWithUK(const tPoseR & aPose) :
81-
mPose (aPose),
82-
mOmega (0.0,0.0,0.0)
172+
mTr (aPose.Tr()),
173+
mRUK (aPose.Rot())
83174
{
84175
}
85176

177+
86178
cPoseWithUK::cPoseWithUK() :
87179
cPoseWithUK(tPoseR())
88180
{
89181
}
90182

183+
184+
91185
cPoseWithUK::~cPoseWithUK()
92186
{
93187
OUK_Reset();
94188
}
95189

96190
void cPoseWithUK::SetPose(const tPoseR & aPose)
97191
{
98-
mPose = aPose;
192+
mRUK.SetRot(aPose.Rot());
193+
mTr = aPose.Tr();
194+
}
195+
196+
197+
tPoseR cPoseWithUK::Pose() const
198+
{
199+
return tPoseR(mTr,mRUK.Rot());
99200
}
100201

101-
const tPoseR & cPoseWithUK::Pose() const {return mPose;}
102-
tPoseR & cPoseWithUK::Pose() {return mPose;}
103-
const cPt3dr & cPoseWithUK::Center() const {return mPose.Tr();}
104-
cPt3dr & cPoseWithUK::Center() {return mPose.Tr();}
105-
cPt3dr cPoseWithUK::AxeI() const {return mPose.Rot().AxeI();}
106-
cPt3dr cPoseWithUK::AxeJ() const {return mPose.Rot().AxeJ();}
107-
cPt3dr cPoseWithUK::AxeK() const {return mPose.Rot().AxeK();}
108-
const cPt3dr & cPoseWithUK::Tr() const {return mPose.Tr();}
109-
cPt3dr & cPoseWithUK::Tr() {return mPose.Tr();}
202+
const tRotR & cPoseWithUK::Rot() const {return mRUK.Rot();}
203+
204+
205+
206+
const cPt3dr & cPoseWithUK::Tr() const {return mTr;}
207+
cPt3dr & cPoseWithUK::GetRefTr() {return mTr;}
208+
209+
cPt3dr cPoseWithUK::AxeI() const {return mRUK.AxeI();}
210+
cPt3dr cPoseWithUK::AxeJ() const {return mRUK.AxeJ();}
211+
cPt3dr cPoseWithUK::AxeK() const {return mRUK.AxeK();}
110212

111213

112214

113-
cPt3dr & cPoseWithUK::Omega() {return mOmega;}
114-
const cPt3dr & cPoseWithUK::Omega() const {return mOmega;}
215+
const cPt3dr & cPoseWithUK::Omega() const {return mRUK.Omega();}
216+
cPt3dr & cPoseWithUK::GetRefOmega() {return mRUK.GetRefOmega();}
217+
218+
void cPoseWithUK::SetOmega(const cPt3dr & anOmega) {mRUK.SetOmega(anOmega);}
115219

116220

117221
/* Let R be the rotation of pose P=(C,R) = : Cam-> Word, what is optimized in colinearity for a ground point G
@@ -143,43 +247,42 @@ const cPt3dr & cPoseWithUK::Omega() const {return mOmega;}
143247
* aMat(1,0) = -aW.z(); aMat(2,0) = aW.y(); aMat(2,1) = -aW.x();
144248
*/
145249

146-
cPt3dr cPoseWithUK::ValAxiatorFixRot(const cRotation3D<tREAL8> & aRotFix) const
250+
251+
cPt3dr cPoseWithUK::ValAxiatorFixRot(const tRotR & aRotFix) const
147252
{
253+
return mRUK.ValAxiatorFixRot(aRotFix);
254+
/*
148255
cDenseMatrix<tREAL8> aM = mPose.Rot().Mat().Transpose() * aRotFix.Mat();
149256
tREAL8 aZ = ( aM(1,0) - aM(0,1)) / 2.0;
150257
tREAL8 aY = (-aM(2,0) + aM(0,2)) / 2.0;
151258
tREAL8 aX = ( aM(2,1) - aM(1,2)) / 2.0;
152259
153260
return cPt3dr(aX,aY,aZ);
261+
*/
154262
}
263+
#if (0)
264+
265+
#endif
155266

156267
void cPoseWithUK::OnUpdate()
157268
{
158-
// used above formula to modify rotation
159-
mPose.SetRotation(mPose.Rot() * cRotation3D<tREAL8>::RotFromAxiator(-mOmega));
160-
// now this have modify rotation, the "delta" is void :
161-
mOmega = cPt3dr(0,0,0);
269+
mRUK.OnUpdate();
162270
}
163271

164272
void cPoseWithUK::FillGetAdrInfoParam(cGetAdrInfoParam<tREAL8> & aGAIP)
165273
{
166-
aGAIP.TestParam(this, &( mPose.Tr().x()),"Cx");
167-
aGAIP.TestParam(this, &( mPose.Tr().y()),"Cy");
168-
aGAIP.TestParam(this, &( mPose.Tr().z()),"Cz");
169-
170-
aGAIP.TestParam(this, &( mOmega.x()) ,"Wx");
171-
aGAIP.TestParam(this, &( mOmega.y()) ,"Wy");
172-
aGAIP.TestParam(this, &( mOmega.z()) ,"Wz");
274+
aGAIP.TestParam(this, &( mTr.x()),"Cx");
275+
aGAIP.TestParam(this, &( mTr.y()),"Cy");
276+
aGAIP.TestParam(this, &( mTr.z()),"Cz");
173277

278+
mRUK.FillGetAdrInfoParam(aGAIP);
174279
SetNameTypeId(aGAIP);
175280
}
176281

177282
void cPoseWithUK::PutUknowsInSpecifiedSetInterval(cSetInterUK_MultipeObj<tREAL8> * aSetInterv)
178283
{
179-
//StdOut() << " *PUK0 :PutUknowsInSetIntervalPutUknowsInSetInterval " << mIndUk0 << " " << mIndUk1 << "\n";
180-
aSetInterv->AddOneInterv(mPose.Tr());
181-
aSetInterv->AddOneInterv(mOmega);
182-
//StdOut() << " *PUK1 PutUknowsInSetIntervalPutUknowsInSetInterval " << mIndUk0 << " " << mIndUk1 << "\n";
284+
aSetInterv->AddOneInterv(mTr);
285+
mRUK.PutUknowsInSpecifiedSetInterval(aSetInterv);
183286
}
184287

185288
void cPoseWithUK::PutUknowsInSetInterval()
@@ -189,11 +292,18 @@ void cPoseWithUK::PutUknowsInSetInterval()
189292

190293
void AddData(const cAuxAr2007 & anAux,cPoseWithUK & aPUK)
191294
{
192-
MMVII::AddData(anAux,aPUK.Pose());
295+
// StdOut() << "vv765vvVVvv void AddData(const cAuxAr2007 & anAux,cPoseWithUK & aPUK)\n";
296+
297+
tPoseR aPose = aPUK.Pose();
298+
MMVII::AddData(anAux,aPose);
299+
if (anAux.Input())
300+
aPUK.SetPose(aPose);
301+
302+
// MMVII::AddData(anAux,aPUK.Pose());
193303

194304
if (anAux.Input())
195305
{
196-
aPUK.Omega() = cPt3dr(0,0,0);
306+
aPUK.SetOmega(cPt3dr(0,0,0));
197307
}
198308
else
199309
{
@@ -203,10 +313,13 @@ void AddData(const cAuxAr2007 & anAux,cPoseWithUK & aPUK)
203313

204314
void cPoseWithUK::PushObs(std::vector<double> & aVObs,bool TransposeMatr)
205315
{
316+
mRUK.PushObs(aVObs,TransposeMatr);
317+
/*
206318
if (TransposeMatr)
207319
mPose.Rot().Mat().PushByCol(aVObs);
208320
else
209321
mPose.Rot().Mat().PushByLine(aVObs);
322+
*/
210323
}
211324

212325
/* ******************************************************* */
@@ -433,17 +546,17 @@ const cPixelDomain & cSensorCamPC::PixelDomain() const
433546
cPerspCamIntrCalib * cSensorCamPC::InternalCalib() const {return mInternalCalib;}
434547

435548
const cPt3dr & cSensorCamPC::Center() const {return mPose_WU.Tr();}
436-
const tRotR & cSensorCamPC::Orient() const {return mPose_WU.Pose().Rot();}
549+
const tRotR & cSensorCamPC::Orient() const {return mPose_WU.Rot();}
437550

438551
const cPt3dr & cSensorCamPC::Omega() const {return mPose_WU.Omega();}
439-
cPt3dr & cSensorCamPC::Center() {return mPose_WU.Tr();}
440-
cPt3dr & cSensorCamPC::Omega() {return mPose_WU.Omega();}
552+
cPt3dr & cSensorCamPC::Center() {return mPose_WU.GetRefTr();}
553+
cPt3dr & cSensorCamPC::Omega() {return mPose_WU.GetRefOmega();}
441554
cPt3dr cSensorCamPC::PseudoCenterOfProj() const {return Center();}
442555

443556
cPt3dr cSensorCamPC::AxeI() const {return mPose_WU.AxeI();}
444557
cPt3dr cSensorCamPC::AxeJ() const {return mPose_WU.AxeJ();}
445558
cPt3dr cSensorCamPC::AxeK() const {return mPose_WU.AxeK();}
446-
const cIsometry3D<tREAL8> & cSensorCamPC::Pose() const {return mPose_WU.Pose();}
559+
tPoseR cSensorCamPC::Pose() const {return mPose_WU.Pose();}
447560

448561
cPoseWithUK & cSensorCamPC::Pose_WU() {return mPose_WU;}
449562

0 commit comments

Comments
 (0)