Skip to content

Commit 5436b87

Browse files
author
deseilligny
committed
Formulas_CamStenope.h : formal line equation
1 parent 6d56616 commit 5436b87

File tree

5 files changed

+93
-15
lines changed

5 files changed

+93
-15
lines changed

‎MMVII/include/MMVII_Ptxd.h‎

Lines changed: 20 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -403,7 +403,26 @@ template <class T,const int Dim> typename tNumTrait<T>::tFloatAssoc Norm2(const
403403
}
404404
// template <class T,const int Dim> double Norm2(const cPtxd<T,Dim> & aP);
405405

406-
template <class T,const int Dim> typename tNumTrait<T>::tBig Scal(const cPtxd<T,Dim> &,const cPtxd<T,Dim> &);
406+
//template <class T,const int Dim> typename tNumTrait<T>::tBig Scal(const cPtxd<T,Dim> &,const cPtxd<T,Dim> &);
407+
template <class T> typename tNumTrait<T>::tBig Scal(const cPtxd<T,1> &aP1,const cPtxd<T,1> & aP2)
408+
{ return aP1.x() * aP2.x();}
409+
template <class T> typename tNumTrait<T>::tBig Scal(const cPtxd<T,2> &aP1,const cPtxd<T,2> & aP2)
410+
{ return aP1.x() * aP2.x() + aP1.y() * aP2.y();}
411+
template <class T> typename tNumTrait<T>::tBig Scal(const cPtxd<T,3> &aP1,const cPtxd<T,3> & aP2)
412+
{ return aP1.x() * aP2.x() + aP1.y() * aP2.y() + aP1.z() * aP2.z();}
413+
template <class T> typename tNumTrait<T>::tBig Scal(const cPtxd<T,4> &aP1,const cPtxd<T,4> & aP2)
414+
{ return aP1.x() * aP2.x() + aP1.y() * aP2.y() + aP1.z() * aP2.z() +aP1.t() * aP2.t();}
415+
/*
416+
template <class T,const int Dim>
417+
typename tNumTrait<T>::tBig Scal(const cPtxd<T,Dim> &aP1,const cPtxd<T,Dim> & aP2)
418+
{
419+
typename tNumTrait<T>::tBig aRes = aP1[0]*aP2[0];
420+
for (int aD=1 ; aD<Dim; aD++)
421+
aRes += aP1[aD]*aP2[aD];
422+
return aRes;
423+
}
424+
*/
425+
407426
template <class T,const int Dim> typename tNumTrait<T>::tBig MulCoord(const cPtxd<T,Dim> & aP);
408427

409428
template <class T,const int Dim> T Cos(const cPtxd<T,Dim> &,const cPtxd<T,Dim> &);

‎MMVII/src/Geoms/PtsBox.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -504,6 +504,7 @@ template <class Type,const int Dim> Type MinAbsCoord(const cPtxd<Type,Dim> & aPt
504504
}
505505

506506

507+
/*
507508
template <class T,const int Dim>
508509
typename tNumTrait<T>::tBig Scal(const cPtxd<T,Dim> &aP1,const cPtxd<T,Dim> & aP2)
509510
{
@@ -512,6 +513,7 @@ template <class T,const int Dim>
512513
aRes += aP1[aD]*aP2[aD];
513514
return aRes;
514515
}
516+
*/
515517

516518
template <class T,const int Dim>
517519
typename tNumTrait<T>::tBig MulCoord(const cPtxd<T,Dim> &aPt)
@@ -1412,7 +1414,7 @@ template double NormK(const cPtxd<TYPE,DIM> & aPt,double anExp);\
14121414
template TYPE Norm1(const cPtxd<TYPE,DIM> & aPt);\
14131415
template TYPE NormInf(const cPtxd<TYPE,DIM> & aPt);\
14141416
template TYPE MinAbsCoord(const cPtxd<TYPE,DIM> & aPt);\
1415-
template typename tNumTrait<TYPE>::tBig Scal(const cPtxd<TYPE,DIM> &,const cPtxd<TYPE,DIM> &);\
1417+
/*template typename tNumTrait<TYPE>::tBig Scal(const cPtxd<TYPE,DIM> &,const cPtxd<TYPE,DIM> &);*/\
14161418
template typename tNumTrait<TYPE>::tBig MulCoord(const cPtxd<TYPE,DIM> &);\
14171419
template TYPE Cos(const cPtxd<TYPE,DIM> &,const cPtxd<TYPE,DIM> &);\
14181420
template TYPE CosWDef(const cPtxd<TYPE,DIM> &,const cPtxd<TYPE,DIM> &,const TYPE&);\

‎MMVII/src/SymbDerGen/ComonHeaderSymb.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -245,7 +245,7 @@ template <typename tScal> cPtxd<tScal,2> VtoP2(const std::vector<tScal> & aV,
245245
{
246246
return cPtxd<tScal,2>(aV.at(aInd),aV.at(aInd+1));
247247
}
248-
template <typename tScal> cPtxd<tScal,2> VtoP2AnutoIncr(const std::vector<tScal> & aV,size_t *aInd)
248+
template <typename tScal> cPtxd<tScal,2> VtoP2AutoIncr(const std::vector<tScal> & aV,size_t *aInd)
249249
{
250250
return VtoP2(aV,IndexAutoIncr(aInd,2));
251251
}

‎MMVII/src/SymbDerGen/Formulas_CamStenope.h‎

Lines changed: 68 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -628,9 +628,12 @@ template <typename TypeDist,typename TypeProj> class cEqColinearityCamPPC
628628
}
629629
std::vector<std::string> VNamesUnknowns() const
630630
{
631+
std::vector<std::string> aVUkGround = mLine ?
632+
Append(NamesP2("N1Coords"),NamesP2("N2Coords")) :
633+
NamesP3("PGround") ;
631634
return Append
632635
(
633-
NamesP3("PGround"), // 0-3
636+
aVUkGround,
634637
NamesPose("CCam","W"), // 3-9
635638
NamesIntr(""), // 9-12
636639
mDist.VNamesParams()
@@ -639,6 +642,19 @@ template <typename TypeDist,typename TypeProj> class cEqColinearityCamPPC
639642

640643
std::vector<std::string> VNamesObs() const
641644
{
645+
if (mLine)
646+
{
647+
std::vector<std::string> aVecLIne2D = Append(NamesP2("Line2D_Pt"),NamesP2("Line2D_Norm"));
648+
649+
std::vector<std::string> aVPtsLine = Append(NamesP3("Line3d_Pt1"),NamesP3("Line3d_Pt2"));
650+
std::vector<std::string> aVPtsNormLine = Append(NamesP3("Line3d_Norm_x"),NamesP3("Line3d_Norm_y"));
651+
std::vector<std::string> aVLambdaLine {"Line3d_Lambda"};
652+
653+
std::vector<std::string> aVecLIne3D = Append(aVPtsLine,aVPtsNormLine,aVLambdaLine);
654+
655+
656+
return Append(aVecLIne2D,aVecLIne3D,NamesMatr("M",cPt2di(3,3)));
657+
}
642658
return Append(NamesP2("Im"),NamesMatr("M",cPt2di(3,3)));
643659
}
644660

@@ -649,15 +665,43 @@ template <typename TypeDist,typename TypeProj> class cEqColinearityCamPPC
649665
const std::vector<tUk> & aVObs
650666
) const
651667
{
668+
tUk aC1 = CreateCste(1.0,aVUk.at(0));
669+
cPtxd<tUk,3> aPGround;
670+
size_t aIndUk = 0;
671+
size_t aIndObs = 0;
672+
673+
cPtxd<tUk,2> aPtIm = VtoP2AutoIncr(aVObs,&aIndObs);
674+
cPtxd<tUk,2> aPtNormIm;
675+
if (mLine)
676+
{
677+
aPtNormIm = VtoP2AutoIncr(aVObs,&aIndObs);
678+
679+
cPtxd<tUk,3> aP3d_1 = VtoP3AutoIncr(aVObs,&aIndObs);
680+
cPtxd<tUk,3> aP3d_2 = VtoP3AutoIncr(aVObs,&aIndObs);
681+
682+
cPtxd<tUk,3> aNorm3d_x = VtoP3AutoIncr(aVObs,&aIndObs);
683+
cPtxd<tUk,3> aNorm3d_y = VtoP3AutoIncr(aVObs,&aIndObs);
684+
tUk aLambda = aVObs.at(aIndObs++);
685+
686+
cPtxd<tUk,2> aWeightN_1 = VtoP2AutoIncr(aVUk,&aIndUk);
687+
cPtxd<tUk,2> aWeightN_2 = VtoP2AutoIncr(aVUk,&aIndUk);
688+
689+
cPtxd<tUk,3> aQ1 = aP3d_1 + aNorm3d_x * aWeightN_1.x() + aNorm3d_y * aWeightN_1.y() ;
690+
cPtxd<tUk,3> aQ2 = aP3d_2 + aNorm3d_x * aWeightN_2.x() + aNorm3d_y * aWeightN_2.y() ;
691+
692+
aPGround = aQ1 * (aC1-aLambda) + aQ2 * aLambda ;
693+
}
652694
// extract unknown parameters from vector
653-
cPtxd<tUk,3> aPGround = VtoP3(aVUk,0);
654-
cPtxd<tUk,3> aCCcam = VtoP3(aVUk,3);
655-
cPtxd<tUk,3> aW = VtoP3(aVUk,6);
656-
tUk aFoc = aVUk.at(9);
657-
cPtxd<tUk,2> aPP = VtoP2(aVUk,10);
695+
else
696+
{
697+
// aPGround = VtoP3(aVUk,0);
698+
aPGround = VtoP3AutoIncr(aVUk,&aIndUk);
699+
}
700+
cPtxd<tUk,3> aCCcam = VtoP3AutoIncr(aVUk,&aIndUk);
701+
cPtxd<tUk,3> aW = VtoP3AutoIncr(aVUk,&aIndUk);
702+
tUk aFoc = aVUk.at(aIndUk++);
703+
cPtxd<tUk,2> aPP = VtoP2AutoIncr(aVUk,&aIndUk);
658704

659-
// obs pixel
660-
cPtxd<tUk,2> aPtIm = VtoP2(aVObs,0);
661705

662706
cPtxd<tUk,3> aVCP = aPGround - aCCcam; // vector CenterCam -> PGround
663707

@@ -669,7 +713,10 @@ template <typename TypeDist,typename TypeProj> class cEqColinearityCamPPC
669713
*/
670714

671715
#else
672-
cMatF<tUk> aRotInit (3,3,aVObs,2);
716+
// cMatF<tUk> aRotInit (3,3,aVObs,2);
717+
cMatF<tUk> aRotInit (3,3,&aIndObs,aVObs);
718+
// cMatF(size_t aSzX,size_t aSzY, size_t * anIndAutoIncr, const std::vector<Type> & aVal) :
719+
673720
cMatF<tUk> aDeltaRot = cMatF<tUk>::MatAxiator(aW);
674721
cPtxd<tUk,3> aPCam = aDeltaRot * (aRotInit * aVCP);
675722
#endif
@@ -679,9 +726,19 @@ template <typename TypeDist,typename TypeProj> class cEqColinearityCamPPC
679726

680727
cPtxd<tUk,2> aPPix = aPP + aPDist * aFoc; // Use Focal and PP to make pixel
681728

682-
cPtxd<tUk,2> aResidual = aPPix - aPtIm; // compare to mesured point
683729

684-
return {aResidual.x(),aResidual.y()};
730+
MMVII_INTERNAL_ASSERT_always(aIndUk+mDist.VNamesParams().size()==aVUk.size(),"cEqColinearityCamPPC : Uk-size");
731+
MMVII_INTERNAL_ASSERT_always(aIndObs== aVObs.size(),"cEqColinearityCamPPC : Obs-size");
732+
733+
cPtxd<tUk,2> aResidual = aPPix - aPtIm; // compare to mesured point
734+
if (mLine)
735+
{
736+
return {Scal(aPtNormIm,aResidual)};
737+
}
738+
else
739+
{
740+
return {aResidual.x(),aResidual.y()};
741+
}
685742
}
686743

687744
TypeDist mDist;

‎MMVII/src/SymbDerGen/Formulas_Lidar.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ class cRadiomLidarIma
6969
cPtxd<tObs,3> aGradProjJ = VtoP3AutoIncr(aVObs,&aIndObs); // J(ordinate) of gradient / PCamera of projection
7070

7171
tUk aRadiomInit = aVObs.at(aIndObs++); // extract the radiometry of image
72-
cPtxd<tObs,2> aGradIm = VtoP2AnutoIncr(aVObs,&aIndObs); // extract the gradient of image
72+
cPtxd<tObs,2> aGradIm = VtoP2AutoIncr(aVObs,&aIndObs); // extract the gradient of image
7373

7474
// compute the position of the point in camera coordinates
7575
cPtxd<tUk,3> aVCP = aPGround - aCCam; // "vector" Center -> PGround

0 commit comments

Comments
 (0)