Skip to content

Commit

Permalink
Master with "whole", ready for tuto on Lidar/Pghr
Browse files Browse the repository at this point in the history
  • Loading branch information
deseilligny committed Jan 19, 2025
1 parent 3ca11b9 commit 793a1b4
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 95 deletions.
77 changes: 12 additions & 65 deletions MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,10 @@ cBA_LidarPhotogra::cBA_LidarPhotogra(cMMVII_BundleAdj& aBA,const std::vector<std
}
}

// Creation of the patches, to comment ...
if (1)
{
/*
cTplBoxOfPts<tREAL8,2> aBoxObj;
int aNbPtsByPtch = 32;
Expand Down Expand Up @@ -109,6 +111,7 @@ cBA_LidarPhotogra::cBA_LidarPhotogra(cMMVII_BundleAdj& aBA,const std::vector<std
<< " NbPts=" << mTri.NbPts() << " => " << aCpt
<< " NbPatch=" << mLPatches.size() << " NbAvg => " << aCpt / double(mLPatches.size())
<< "\n";
*/
}
}

Expand All @@ -125,8 +128,6 @@ void cBA_LidarPhotogra::AddObs(tREAL8 aW)
{
for (size_t aKP=0 ; aKP<mTri.NbPts() ; aKP++)
{
// cPt3df aPF = mTri.KthPts(aKP);
// AddObs(aW,cPt3dr(aPF.x(),aPF.y(),aPF.z()));
Add1Patch(aW,{ToR(mTri.KthPts(aKP))});
}
}
Expand Down Expand Up @@ -155,41 +156,20 @@ void cBA_LidarPhotogra::SetVUkVObs
int aKPt
)
{

cSensorCamPC * aCam = mVCam.at(aData.mKIm);
cPt3dr aPCam = aCam->Pt_W2L(aPGround);
tProjImAndGrad aPImGr = aCam->InternalCalib()->DiffGround2Im(aPCam);

if (aVIndUk)
{
// *aVIndUk = std::vector<int> {-1} ; // Index for temporary radiom
aCam->PushIndexes(*aVIndUk);
}

cPoseWithUK& aPUK = aCam->Pose_WU();


aPUK.PushObs(aVObs,true); // true this is the W->C, wich the transposition of IJK : C->W
aPGround.PushInStdVector(aVObs);

aPCam.PushInStdVector(aVObs);
aPImGr.mGradI.PushInStdVector(aVObs);
aPImGr.mGradJ.PushInStdVector(aVObs);

aVObs.push_back(aData.mVGr.at(aKPt).first);
aData.mVGr.at(aKPt).second.PushInStdVector(aVObs);
// to complete ....
}



void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector<cPt3dr> & aVPatchGr)
{
cResolSysNonLinear<tREAL8> * aSys = mBA.Sys();
// cResolSysNonLinear<tREAL8> * aSys = mBA.Sys();
std::vector<cData1ImLidPhgr> aVData;
cWeightAv<tREAL8,tREAL8> aWAv;

cComputeStdDev<tREAL8> aStdDev;

// to comment .....
for (size_t aKIm=0 ; aKIm<mVCam.size() ; aKIm++)
{
cSensorCamPC * aCam = mVCam[aKIm];
Expand Down Expand Up @@ -223,7 +203,6 @@ void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector<cPt3dr> & aV
}
}


if (aVData.size()<2) return;

mLastResidual.Add(1.0, (aStdDev.StdDev(1e-5) *aVData.size()) / (aVData.size()-1.0));
Expand All @@ -232,44 +211,12 @@ void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector<cPt3dr> & aV

if (mNumMode==0)
{
std::vector<tREAL8> aVTmpAvg({aWAv.Average()});
cSetIORSNL_SameTmp<tREAL8> aStrSubst(aVTmpAvg);
for (const auto & aData : aVData)
{
std::vector<int> aVIndUk{-1} ;
std::vector<tREAL8> aVObs;
SetVUkVObs(aVPatchGr.at(0),&aVIndUk,aVObs,aData,0);
aSys->R_AddEq2Subst(aStrSubst,mEqLidPhgr,aVIndUk,aVObs,aWeight);
}
aSys->R_AddObsWithTmpUK(aStrSubst);
}
else if (mNumMode==1)
{
for (size_t aKPt=1; aKPt<aVPatchGr.size() ; aKPt++)
{
cWeightAv<tREAL8,tREAL8> aAvRatio;
for (const auto & aData : aVData)
{
tREAL8 aV0 = aData.mVGr.at(0).first;
tREAL8 aVK = aData.mVGr.at(aKPt).first;
aAvRatio.Add(1.0,NormalisedRatioPos(aV0,aVK)) ;
}
std::vector<tREAL8> aVTmpAvg({aAvRatio.Average()});
cSetIORSNL_SameTmp<tREAL8> aStrSubst(aVTmpAvg);
for (const auto & aData : aVData)
{
std::vector<int> aVIndUk{-1} ;
std::vector<tREAL8> aVObs;

SetVUkVObs(aVPatchGr.at(0) ,&aVIndUk,aVObs,aData,0);
SetVUkVObs(aVPatchGr.at(aKPt),nullptr ,aVObs,aData,aKPt);
aSys->R_AddEq2Subst(aStrSubst,mEqLidPhgr,aVIndUk,aVObs,aWeight);
}
aSys->R_AddObsWithTmpUK(aStrSubst);
}
}


// to complete ....
}
else if (mNumMode==1)
{
// to complete ...
}
}

};
35 changes: 5 additions & 30 deletions MMVII/src/SymbDerGen/Formulas_Lidar.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,33 +57,18 @@ class cRadiomLidarIma
size_t & aIndObs
) const
{
// to complete

// read the unknowns
cPtxd<tUk,3> aCCcam = VtoP3AuoIncr(aVUk,&aIndUk);
cPtxd<tUk,3> aW = VtoP3AuoIncr(aVUk,&aIndUk);

// read the observation
cMatF<tUk> aRotInit (3,3,&aIndObs,aVObs); // Curent value of rotatuin

cPtxd<tUk,3> aPGround = VtoP3AuoIncr(aVObs,&aIndObs); // PGround
cPtxd<tUk,3> aPCamInit = VtoP3AuoIncr(aVObs,&aIndObs); // Current value of PGround in camera coordinate
cPtxd<tUk,3> aGradI = VtoP3AuoIncr(aVObs,&aIndObs); // gradient / PCam of abscisse of projection in image
cPtxd<tUk,3> aGradJ = VtoP3AuoIncr(aVObs,&aIndObs); // gradient / PCam of ordonate of projection in image

tUk aRadiom0 = aVObs.at(aIndObs++); // radiometry in image of current proj
cPtxd<tUk,2> aGradR = VtoP2AuoIncr(aVObs,&aIndObs); // gradient of radiometry in image

// compute the position of the point in camera coordinates
cPtxd<tUk,3> aVCP = aPGround - aCCcam; // vector CamCenter -> PGround
cMatF<tUk> aDeltaRot = cMatF<tUk>::MatAxiator(aW); // unknown "small rotation"
cPtxd<tUk,3> aPCam = aDeltaRot * (aRotInit * aVCP); // point in camera coordinate, taking into account the unknowns

// compute the position of projected point
cPtxd<tUk,3> aDPCam = aPCam-aPCamInit; // difference Unknown/Curent of point in camera coordinate
tUk aDI = PScal(aGradI,aDPCam); // variation of abscissa of projection
tUk aDJ = PScal(aGradJ,aDPCam); // variation of ordinate of projection

// compute the radiometry
return aRadiom0 + PScal(aGradR,cPtxd<tUk,2>(aDI,aDJ));
return 0;
}

static std::vector<std::string> NamesPoseUK() {return Append(NamesP3("mCCam"),NamesP3("mOmega"));}
Expand Down Expand Up @@ -154,19 +139,9 @@ class cEqLidarImCensus : public cRadiomLidarIma
const std::vector<tObs> & aVObs
) const
{
// read the unknowns
size_t aIndUk = 0;
size_t aIndObs = 0;

tUk aRatioTarget = aVUk.at(aIndUk++);
// to complete

tUk aRadiom0 = Radiom_PerpCentrIntrFix(aVUk,aIndUk,aVObs,aIndObs);
aIndUk = 1;
tUk aRadiom1 = Radiom_PerpCentrIntrFix(aVUk,aIndUk,aVObs,aIndObs);


// return {NormalisedRatioPos(aRadiom , aRadiomTarget)};
return {NormalisedRatioPos(aRadiom0,aRadiom1) - aRatioTarget};
return {};
}

std::vector<std::string> VNamesUnknowns() const {return Append({"TargetRatio"},NamesPoseUK());}
Expand Down

0 comments on commit 793a1b4

Please sign in to comment.