diff --git a/MMVII/src/BundleAdjustment/BundleAdjustment.h b/MMVII/src/BundleAdjustment/BundleAdjustment.h index 55cddd894..1cf0630c6 100644 --- a/MMVII/src/BundleAdjustment/BundleAdjustment.h +++ b/MMVII/src/BundleAdjustment/BundleAdjustment.h @@ -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> mVGr; + size_t mKIm; // num of images where the patch is seen + std::vector> mVGr; // pair of radiometry/gradient values in each image for each point of the patch }; diff --git a/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp b/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp index f4f9df401..9faac3f3b 100644 --- a/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp +++ b/MMVII/src/BundleAdjustment/Bundle_LidarPhotogra.cpp @@ -6,6 +6,8 @@ namespace MMVII { +/** Class for geometrically indexing the lidars (on 2D point) for patches creation */ + template class cTil2DTri3D { public : @@ -25,28 +27,30 @@ template class cTil2DTri3D cBA_LidarPhotogra::cBA_LidarPhotogra(cMMVII_BundleAdj& aBA,const std::vector& aParam) : - mBA (aBA), - mNumMode (cStrIO::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::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 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::FromFile(aPtrCam->NameImage())); + mVCam.push_back(aPtrCam->GetSensorCamPC()); // yes get it + mVIms.push_back(cIm2D::FromFile(aPtrCam->NameImage())); // read the image } else { @@ -133,6 +137,7 @@ void cBA_LidarPhotogra::AddObs(tREAL8 aW) } else { + MMVII_UnclasseUsEr("Dont handle Census"); for (const auto& aPatchIndex : mLPatches) { std::vector aVP; @@ -156,62 +161,98 @@ void cBA_LidarPhotogra::SetVUkVObs int aKPt ) { - // to complete .... } void cBA_LidarPhotogra::Add1Patch(tREAL8 aWeight,const std::vector & aVPatchGr) { - // cResolSysNonLinear * aSys = mBA.Sys(); - std::vector aVData; - cWeightAv aWAv; + // read the solver now, because was not initialized at creation + cResolSysNonLinear * aSys = mBA.Sys(); - cComputeStdDev aStdDev; + std::vector aVData; // for each image where patch is visible will store the data + cWeightAv aWAv; // compute average of image for radiom unknown + cComputeStdDev 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 & aDIm = mVIms[aKIm].DIm(); + cSensorCamPC * aCam = mVCam[aKIm]; // extract cam + cDataIm2D & 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 ; aKPtIsVisible(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 aVTmpAvg{aWAv.Average()}; // vector for initializingz the temporay (here 1 = average) + cSetIORSNL_SameTmp 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 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 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) { diff --git a/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp b/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp index 7fa9650c4..eba50e5c1 100644 --- a/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp +++ b/MMVII/src/BundleAdjustment/cAppliBundAdj.cpp @@ -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") diff --git a/MMVII/src/SymbDerGen/Formulas_Lidar.h b/MMVII/src/SymbDerGen/Formulas_Lidar.h index 8b1ef72bc..7c21f3efd 100755 --- a/MMVII/src/SymbDerGen/Formulas_Lidar.h +++ b/MMVII/src/SymbDerGen/Formulas_Lidar.h @@ -60,15 +60,34 @@ class cRadiomLidarIma // to complete // read the unknowns - + cPtxd aCCam = VtoP3AuoIncr(aVUk,&aIndUk); // camera center + cPtxd aW = VtoP3AuoIncr(aVUk,&aIndUk); // camera infinitesimal rotation + // read the observation + cMatF aRotInit (3,3,&aIndObs,aVObs); // Curent value of rotation + cPtxd aPGround = VtoP3AuoIncr(aVObs,&aIndObs); // Value of 3D ground point + cPtxd aPCamInit = VtoP3AuoIncr(aVObs,&aIndObs); // Current value of 3D point in camera system + cPtxd aGradProjI = VtoP3AuoIncr(aVObs,&aIndObs); // I(abscissa) of gradient / PCamera of projection + cPtxd 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 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 aVCP = aPGround - aCCam; // "vector" Center -> PGround + cMatF aDeltaRot = cMatF::MatAxiator(aW); // small rotation associated to W + cPtxd aPCoordCam = aDeltaRot * aRotInit * aVCP; + + // d Intr + // Intr(Pose(Pground)) = Intr(PCam0) + ------- * (Pose(Pground) - PCam0) + // dcam + // + cPtxd 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(aDelta_I,aDelta_J)); } static std::vector NamesPoseUK() {return Append(NamesP3("mCCam"),NamesP3("mOmega"));} @@ -80,11 +99,18 @@ class cRadiomLidarIma static std::vector 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 VectObsRadiom() { + // Radiom + grad /i,j return Append({"Rad0"},NamesP2("GradRad")); } }; @@ -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 VNamesUnknowns() const {return Append({"TargetRad"},NamesPoseUK());}