Skip to content

Commit

Permalink
In exo Lidar/Phgr, done ponct
Browse files Browse the repository at this point in the history
  • Loading branch information
deseilligny committed Jan 20, 2025
1 parent 19bd3c5 commit 8d45593
Show file tree
Hide file tree
Showing 4 changed files with 113 additions and 46 deletions.
5 changes: 3 additions & 2 deletions MMVII/src/BundleAdjustment/BundleAdjustment.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -349,11 +349,12 @@ class cBA_TieP
};


/** For a given patch in one image, will store all the data on the points*/
class cData1ImLidPhgr
{
public :
size_t mKIm;
std::vector<std::pair<tREAL8,cPt2dr>> mVGr;
size_t mKIm; // num of images where the patch is seen
std::vector<std::pair<tREAL8,cPt2dr>> mVGr; // pair of radiometry/gradient values in each image for each point of the patch
};


Expand Down
105 changes: 73 additions & 32 deletions MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@ namespace MMVII
{


/** Class for geometrically indexing the lidars (on 2D point) for patches creation */

template <class Type> class cTil2DTri3D
{
public :
Expand All @@ -25,28 +27,30 @@ template <class Type> class cTil2DTri3D


cBA_LidarPhotogra::cBA_LidarPhotogra(cMMVII_BundleAdj& aBA,const std::vector<std::string>& aParam) :
mBA (aBA),
mNumMode (cStrIO<int>::FromStr(aParam.at(0))),
mTri (aParam.at(1)),
// mInterp (new cCubicInterpolator(-0.5)),
mInterp (nullptr),
mEqLidPhgr ( (mNumMode==0) ? EqEqLidarImPonct(true,1) : EqEqLidarImCensus(true,1))
mBA (aBA), // memorize the bundel adj class itself (access to optimizer)
mNumMode (cStrIO<int>::FromStr(aParam.at(0))), // mode of matching (int 4 now) 0 ponct, 1 Census
mTri (aParam.at(1)), // Lidar point themself, stored as a triangulation
mInterp (nullptr), // interpolator see bellow
mEqLidPhgr ( (mNumMode==0) ? EqEqLidarImPonct(true,1) : EqEqLidarImCensus(true,1)) // equation of egalisation Lidar/Phgr
{
//cSinCApodInterpolator aSinC(20,20);
//mInterp = new cTabulatedDiffInterpolator(aSinC,1000);
// By default use tabulation of apodized sinus cardinal
std::vector<std::string> aParamInt {"Tabul","1000","SinCApod","10","10"};
if (aParam.size() >=3)
{
// if specified, take user's param
aParamInt = Str2VStr(aParam.at(2));
}
// create the interpaltor itself
mInterp = cDiffInterpolator1D::AllocFromNames(aParamInt);

// parse the camera and create images
for (const auto aPtrCam : aBA.VSIm())
{
// is it a central perspective camera ?
if (aPtrCam->IsSensorCamPC())
{
mVCam.push_back(aPtrCam->GetSensorCamPC());
mVIms.push_back(cIm2D<tU_INT1>::FromFile(aPtrCam->NameImage()));
mVCam.push_back(aPtrCam->GetSensorCamPC()); // yes get it
mVIms.push_back(cIm2D<tU_INT1>::FromFile(aPtrCam->NameImage())); // read the image
}
else
{
Expand Down Expand Up @@ -133,6 +137,7 @@ void cBA_LidarPhotogra::AddObs(tREAL8 aW)
}
else
{
MMVII_UnclasseUsEr("Dont handle Census");
for (const auto& aPatchIndex : mLPatches)
{
std::vector<cPt3dr> aVP;
Expand All @@ -156,62 +161,98 @@ void cBA_LidarPhotogra::SetVUkVObs
int aKPt
)
{
// to complete ....
}



void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector<cPt3dr> & aVPatchGr)
{
// cResolSysNonLinear<tREAL8> * aSys = mBA.Sys();
std::vector<cData1ImLidPhgr> aVData;
cWeightAv<tREAL8,tREAL8> aWAv;
// read the solver now, because was not initialized at creation
cResolSysNonLinear<tREAL8> * aSys = mBA.Sys();

cComputeStdDev<tREAL8> aStdDev;
std::vector<cData1ImLidPhgr> aVData; // for each image where patch is visible will store the data
cWeightAv<tREAL8,tREAL8> aWAv; // compute average of image for radiom unknown
cComputeStdDev<tREAL8> aStdDev; // compute the standard deviation of projected radiometry (indicator)

// to comment .....
// Parse all the image, we will select the images where all point of a patch are visible
for (size_t aKIm=0 ; aKIm<mVCam.size() ; aKIm++)
{
cSensorCamPC * aCam = mVCam[aKIm];
cDataIm2D<tU_INT1> & aDIm = mVIms[aKIm].DIm();
cSensorCamPC * aCam = mVCam[aKIm]; // extract cam
cDataIm2D<tU_INT1> & aDIm = mVIms[aKIm].DIm(); // extract image

if (aCam->IsVisible(aVPatchGr.at(0)))
if (aCam->IsVisible(aVPatchGr.at(0))) // first test : is central point visible
{
cData1ImLidPhgr aData;
cData1ImLidPhgr aData; // data that will be filled
aData.mKIm = aKIm;
for (size_t aKPt=0 ; aKPt<aVPatchGr.size() ; aKPt++)
for (size_t aKPt=0 ; aKPt<aVPatchGr.size() ; aKPt++) // parse the points of the patch
{
cPt3dr aPGround = aVPatchGr.at(aKPt);
if (aCam->IsVisible(aPGround))
if (aCam->IsVisible(aPGround)) // is the point visible in the camera
{
cPt2dr aPIm = mVCam[aKIm]->Ground2Image(aPGround);
if (aDIm.InsideInterpolator(*mInterp,aPIm,1.0))
cPt2dr aPIm = mVCam[aKIm]->Ground2Image(aPGround); // extract the image projection
if (aDIm.InsideInterpolator(*mInterp,aPIm,1.0)) // is it sufficiently inside
{
auto aVGr = aDIm.GetValueAndGradInterpol(*mInterp,aPIm);
aData.mVGr.push_back(aVGr);
auto aVGr = aDIm.GetValueAndGradInterpol(*mInterp,aPIm); // extract pair Value/Grad of image
aData.mVGr.push_back(aVGr); // push it at end of stack
}
}
}
// Does all the point of the patch were inside the image ?
if (aData.mVGr.size() == aVPatchGr.size())
{
tREAL8 aValIm = aData.mVGr.at(0).first;
aVData.push_back(aData);
aWAv.Add(1.0,aValIm);
aStdDev.Add(1.0,aValIm);
aVData.push_back(aData); // memorize the data for this image

tREAL8 aValIm = aData.mVGr.at(0).first; // value of first/central pixel in this image
aWAv.Add(1.0,aValIm); // compute average
aStdDev.Add(1.0,aValIm); // compute std deviation
}

}
}

// if less than 2 images : nothing valuable to do
if (aVData.size()<2) return;

// accumlulate for computing average of deviation
mLastResidual.Add(1.0, (aStdDev.StdDev(1e-5) *aVData.size()) / (aVData.size()-1.0));



if (mNumMode==0)
{
// to complete ....
cPt3dr aPGround = aVPatchGr.at(0);
std::vector<tREAL8> aVTmpAvg{aWAv.Average()}; // vector for initializingz the temporay (here 1 = average)
cSetIORSNL_SameTmp<tREAL8> aStrSubst(aVTmpAvg); // structure for handling schurr eliminatio,
// parse the data of the patch
for (const auto & aData : aVData)
{
cSensorCamPC * aCam = mVCam.at(aData.mKIm); // extract the camera
cPt3dr aPCam = aCam->Pt_W2L(aPGround); // coordinate of point in image system
tProjImAndGrad aPImGr = aCam->InternalCalib()->DiffGround2Im(aPCam); // compute proj & gradient

// Vector of indexes of unknwons
std::vector<int> aVIndUk{-1} ; // first one is a temporary (convention < 0)
aCam->PushIndexes(aVIndUk); // add the unknowns [C,R] of the camera


// vector that will contains values of observation at this step
std::vector<tREAL8> aVObs;
aCam->Pose_WU().PushObs(aVObs,true); // true because we transpose: we use W->C, which is the transposition of IJK : C->W

aPGround.PushInStdVector(aVObs); //
aPCam.PushInStdVector(aVObs);

aPImGr.mGradI.PushInStdVector(aVObs); // Grad Proj/PCam
aPImGr.mGradJ.PushInStdVector(aVObs);

auto [aRad0,aGradIm] = aData.mVGr.at(0); // Radiom & grad
aVObs.push_back(aRad0);
aGradIm.PushInStdVector(aVObs);

// accumulate the equation involving the radiom
aSys->R_AddEq2Subst(aStrSubst,mEqLidPhgr,aVIndUk,aVObs,aWeight);
}
// do the substitution & add the equation reduced (Schurr complement)
aSys->R_AddObsWithTmpUK(aStrSubst);
}
else if (mNumMode==1)
{
Expand Down
2 changes: 1 addition & 1 deletion MMVII/src/BundleAdjustment/cAppliBundAdj.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ cCollecSpecArg2007 & cAppliBundlAdj::ArgOpt(cCollecSpecArg2007 & anArgOpt)
<< AOpt2007(mGCPFilterAdd,"GCPFilterAdd","Pattern to filter GCP by additional info")
<< AOpt2007(mTiePWeight,"TiePWeight","Tie point weighting [Sig0,SigAtt?=-1,Thrs?=-1,Exp?=1]",{{eTA2007::ISizeV,"[1,4]"}})
<< AOpt2007(mAddTieP,"AddTieP","For additional TieP, [[Folder,SigG...],[Folder,...]] ")
<< AOpt2007(mParamLidarPhgr,"LidarPhotogra","Show names of unknowns (tuning) ")
<< AOpt2007(mParamLidarPhgr,"LidarPhotogra","Paramaters for adj Lidar/Phgr [[Mode,Ply,Interp?]*]")
<< AOpt2007(mPatParamFrozCalib,"PPFzCal","Pattern for freezing internal calibration parameters")
<< AOpt2007(mPatFrosenCenters,"PatFzCenters","Pattern of images for freezing center of poses")
<< AOpt2007(mPatFrosenOrient,"PatFzOrient","Pattern of images for freezing orientation of poses")
Expand Down
47 changes: 36 additions & 11 deletions MMVII/src/SymbDerGen/Formulas_Lidar.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,15 +60,34 @@ class cRadiomLidarIma
// to complete

// read the unknowns

cPtxd<tUk,3> aCCam = VtoP3AuoIncr(aVUk,&aIndUk); // camera center
cPtxd<tUk,3> aW = VtoP3AuoIncr(aVUk,&aIndUk); // camera infinitesimal rotation

// read the observation
cMatF<tObs> aRotInit (3,3,&aIndObs,aVObs); // Curent value of rotation
cPtxd<tObs,3> aPGround = VtoP3AuoIncr(aVObs,&aIndObs); // Value of 3D ground point
cPtxd<tObs,3> aPCamInit = VtoP3AuoIncr(aVObs,&aIndObs); // Current value of 3D point in camera system
cPtxd<tObs,3> aGradProjI = VtoP3AuoIncr(aVObs,&aIndObs); // I(abscissa) of gradient / PCamera of projection
cPtxd<tObs,3> aGradProjJ = VtoP3AuoIncr(aVObs,&aIndObs); // J(ordinate) of gradient / PCamera of projection

// compute the position of the point in camera coordinates
tUk aRadiomInit = aVObs.at(aIndObs++); // extract the radiometry of image
cPtxd<tObs,2> aGradIm = VtoP2AuoIncr(aVObs,&aIndObs); // extract the gradient of image

// compute the position of projected point
// compute the position of the point in camera coordinates
cPtxd<tUk,3> aVCP = aPGround - aCCam; // "vector" Center -> PGround
cMatF<tUk> aDeltaRot = cMatF<tUk>::MatAxiator(aW); // small rotation associated to W
cPtxd<tUk,3> aPCoordCam = aDeltaRot * aRotInit * aVCP;

// d Intr
// Intr(Pose(Pground)) = Intr(PCam0) + ------- * (Pose(Pground) - PCam0)
// dcam
//
cPtxd<tUk,3> aDeltaPCam = aPCoordCam-aPCamInit; // difference Unknown point in cam coord, vs its current value
tUk aDelta_I = PScal(aDeltaPCam,aGradProjI); // scalar product gradient with diff
tUk aDelta_J = PScal(aDeltaPCam,aGradProjJ); // scalar product gradient with diff

// compute the radiometry
return 0;
return aRadiomInit + PScal(aGradIm,cPtxd<tObs,2>(aDelta_I,aDelta_J));
}

static std::vector<std::string> NamesPoseUK() {return Append(NamesP3("mCCam"),NamesP3("mOmega"));}
Expand All @@ -80,11 +99,18 @@ class cRadiomLidarIma

static std::vector<std::string> VectObsPCam()
{
return Append(NamesP3("mPGround"),NamesP3("mPCam0"),NamesP3("mGradPCam_i"),NamesP3("mGradPCam_j")) ;
return Append
(
NamesP3("mPGround"), // Ground point
NamesP3("mPCam0"), // initial current value of PGround in camera system
NamesP3("mGradPCam_i"), // (d PIm / d PCam ).i
NamesP3("mGradPCam_j") // (d PIm / d PCam) .j
) ;
}

static std::vector<std::string> VectObsRadiom()
{
// Radiom + grad /i,j
return Append({"Rad0"},NamesP2("GradRad"));
}
};
Expand All @@ -101,14 +127,13 @@ class cEqLidarImPonct : public cRadiomLidarIma
) const
{
// read the unknowns
// size_t aIndUk = 0;
// size_t aIndObs = 0;
size_t aIndUk = 0;
size_t aIndObs = 0;

//tUk aRadiomTarget = aVUk.at(aIndUk++);
//tUk aRadiom = Radiom_PerpCentrIntrFix(aVUk,aIndUk,aVObs,aIndObs);
tUk aRadiomTarget = aVUk.at(aIndUk++);
tUk aRadiom = Radiom_PerpCentrIntrFix(aVUk,aIndUk,aVObs,aIndObs);

// return {NormalisedRatioPos(aRadiom , aRadiomTarget)};
return {aVUk.at(0)} ; // {aRadiom- aRadiomTarget};
return {aRadiom - aRadiomTarget};
}

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

0 comments on commit 8d45593

Please sign in to comment.