diff --git a/MMVII/include/MMVII_PCSens.h b/MMVII/include/MMVII_PCSens.h index 5cd20d77c5..cca13f0303 100755 --- a/MMVII/include/MMVII_PCSens.h +++ b/MMVII/include/MMVII_PCSens.h @@ -441,6 +441,7 @@ class cSensorCamPC : public cSensorImage void SetPose(const tPose & aPose); + bool HasImageAndDepth() const override; // true cPt3dr Ground2ImageAndDepth(const cPt3dr &) const override; cPt3dr ImageAndDepth2Ground(const cPt3dr & ) const override; tSeg3dr Image2Bundle(const cPt2dr &) const override; diff --git a/MMVII/include/MMVII_Sensor.h b/MMVII/include/MMVII_Sensor.h index 1f7bea878a..82895191a1 100755 --- a/MMVII/include/MMVII_Sensor.h +++ b/MMVII/include/MMVII_Sensor.h @@ -98,6 +98,9 @@ class cSensorImage : public cObj2DelAtEnd, /// Basic method GroundCoordinate -> image coordinate of projection virtual cPt2dr Ground2Image(const cPt3dr &) const = 0; + + /// Can we manipulate Image & Depth -> 3d, default false, true for Central Persp + virtual bool HasImageAndDepth() const; /// add the the depth (to see if have a default with bundle+Gr2Ima), default error virtual cPt3dr Ground2ImageAndDepth(const cPt3dr &) const ; /// Invert of Ground2ImageAndDepth, default error @@ -109,6 +112,10 @@ class cSensorImage : public cObj2DelAtEnd, cPt3dr Ground2ImageAndZ(const cPt3dr &) const ; /// Invert of Ground2ImageAndZ, default use bundle, RPC for ex have a specialize method virtual cPt3dr ImageAndZ2Ground(const cPt3dr &) const ; + /// Does it know its Z-validity interval + virtual bool HasIntervalZ() const; + /// Default error + virtual cPt2dr GetIntervalZ() const; /// Compute 3D point by bundle intersection @@ -131,10 +138,10 @@ class cSensorImage : public cObj2DelAtEnd, /// return a set point regulary sampled (+/-) on sensor, take care of frontier, default is as simple grid virtual std::vector PtsSampledOnSensor(int aNbByDim) const ; - /// return artificial/synthetic correspondance , with vector of depth - cSet2D3D SyntheticsCorresp3D2D (int aNbByDim,std::vector & aVecDepth) const; - /// call variant with vector, depth regularly spaced - cSet2D3D SyntheticsCorresp3D2D (int aNbByDim,int aNbDepts,double aD0,double aD1) const; + /// return artificial/synthetic correspondance , with vector of depth / Z + cSet2D3D SyntheticsCorresp3D2D (int aNbByDim,std::vector & aVecDepth,bool IsDepthOrZ) const; + /// call variant with vector, depth regularly spaced depth / Z + cSet2D3D SyntheticsCorresp3D2D (int aNbByDim,int aNbDepts,double aD0,double aD1,bool IsDepthOrZ) const; // ================= Residual =========================== diff --git a/MMVII/include/MMVII_Sys.h b/MMVII/include/MMVII_Sys.h index 229e0e2eda..f59b5a02f2 100755 --- a/MMVII/include/MMVII_Sys.h +++ b/MMVII/include/MMVII_Sys.h @@ -23,7 +23,8 @@ namespace MMVII #define MMVII_SYS_L 6 // Gnu/Linux, first perfect number ;-) #define MMVII_SYS_A 666 // Apple , Evil's number ;-( -#define MMVII_SYS_W 2610 // Window , Evil's number in non standard hexadecimal , some system like to do it hard ... +#define MMVII_SYS_W 2910 // Window ,Evil's number in non standard hexadecimal ,some system like to do it hard ... 0x29A =10+9*16+2*256=666 + enum class eSYS { diff --git a/MMVII/include/cMMVII_Appli.h b/MMVII/include/cMMVII_Appli.h index b9e59efb65..ea67e12e47 100755 --- a/MMVII/include/cMMVII_Appli.h +++ b/MMVII/include/cMMVII_Appli.h @@ -406,7 +406,7 @@ class cMMVII_Appli : public cMMVII_Ap_NameManip, void MMVII_WARNING(const std::string &); /// According to StdOut param can be std::cout, a File, both or none - cMultipleOfs & StdOut(); + cMultipleOfs & StdOut() const; cMultipleOfs & HelpOut(); cMultipleOfs & ErrOut(); @@ -573,6 +573,9 @@ class cMMVII_Appli : public cMMVII_Ap_NameManip, static const std::string & FullBin(); ///< Protected accessor to full pathname of MMVII executable static const std::string & DirTestMMVII(); ///< Protected accessor to dir to read/write test bench private : + // not very clean, but mutable dont seem enough + cMultipleOfs & NC_StdOut(); + cMMVII_Appli(const cMMVII_Appli&) = delete ; ///< New C++11 feature , forbid copy cMMVII_Appli & operator = (const cMMVII_Appli&) = delete ; ///< New C++11 feature , forbid copy // Subst (aNameOpt,aVal) diff --git a/MMVII/src/Appli/cMMVII_Appli.cpp b/MMVII/src/Appli/cMMVII_Appli.cpp index d6ce0abd4b..4e6bd0c325 100755 --- a/MMVII/src/Appli/cMMVII_Appli.cpp +++ b/MMVII/src/Appli/cMMVII_Appli.cpp @@ -367,7 +367,7 @@ cMultipleOfs& ErrOut() {return StdOut();} -cMultipleOfs & cMMVII_Appli::StdOut() +cMultipleOfs & cMMVII_Appli::NC_StdOut() { /// Maybe mStdCout not correctly initialized if we are in constructor or in destructor ? if ((!cMMVII_Appli::ExistAppli()) || msInDstructor) @@ -377,6 +377,10 @@ cMultipleOfs & cMMVII_Appli::StdOut() cMultipleOfs & cMMVII_Appli::HelpOut() {return StdOut();} cMultipleOfs & cMMVII_Appli::ErrOut() {return StdOut();} +cMultipleOfs & cMMVII_Appli::StdOut() const +{ + return const_cast(this)->NC_StdOut(); +} void TestMainSet(const cCollecSpecArg2007 & aVSpec,bool &aMain0,bool & aMain1) { diff --git a/MMVII/src/PoseEstim/UnCalibratedSpaceResection.cpp b/MMVII/src/PoseEstim/UnCalibratedSpaceResection.cpp index ee1353625f..874f2cbe82 100644 --- a/MMVII/src/PoseEstim/UnCalibratedSpaceResection.cpp +++ b/MMVII/src/PoseEstim/UnCalibratedSpaceResection.cpp @@ -585,7 +585,7 @@ void OneBenchUnCalibResection(int aKTest) cSensorCamPC aCam("Camera_BenchUncalibResection",cIsometry3D::RandomIsom3D(100.0),aCalib); std::vector aVDepts({1,2}); - cSet2D3D aSetCorresp = aCam.SyntheticsCorresp3D2D(10,aVDepts) ; + cSet2D3D aSetCorresp = aCam.SyntheticsCorresp3D2D(10,aVDepts,true) ; cUncalibSpaceRessection aResec8(aSz,aSetCorresp,&aCam); cSensorCamPC * aCamCalc = aResec8.ComputeParameters("NoIm_UCSR"); diff --git a/MMVII/src/Sensors/SensorBases.cpp b/MMVII/src/Sensors/SensorBases.cpp index 62ae278f10..7599df4e90 100644 --- a/MMVII/src/Sensors/SensorBases.cpp +++ b/MMVII/src/Sensors/SensorBases.cpp @@ -172,6 +172,11 @@ void cSensorImage::ToFile(const std::string &) const } +cPt2dr cSensorImage::GetIntervalZ() const +{ + MMVII_INTERNAL_ERROR("cSensorImage::GetIntervalZ not implemanted"); + return cPt2dr::Dummy(); +} @@ -210,6 +215,11 @@ cPt3dr cSensorImage::ImageAndDepth2Ground(const cPt2dr & aP2,const double & aDep return ImageAndDepth2Ground(cPt3dr(aP2.x(),aP2.y(),aDepth)); } +bool cSensorImage::HasImageAndDepth() const {return false;} +bool cSensorImage::HasIntervalZ() const {return false;} + + + const cPt2di & cSensorImage::Sz() const {return PixelDomain().Sz();} @@ -276,7 +286,7 @@ cPt3dr cSensorImage::RandomVisiblePGround(tREAL8 aDepMin,tREAL8 aDepMax) -cSet2D3D cSensorImage::SyntheticsCorresp3D2D (int aNbByDim,std::vector & aVecDepth) const +cSet2D3D cSensorImage::SyntheticsCorresp3D2D (int aNbByDim,std::vector & aVecDepth,bool IsDepthOrZ) const { cSet2D3D aResult; @@ -286,14 +296,17 @@ cSet2D3D cSensorImage::SyntheticsCorresp3D2D (int aNbByDim,std::vector { for (const auto & aDepth : aVecDepth) { - aResult.AddPair(aPIm,ImageAndDepth2Ground(aPIm,aDepth)); + cPt3dr aP = IsDepthOrZ ? + ImageAndDepth2Ground(aPIm,aDepth) : + ImageAndZ2Ground(cPt3dr(aPIm.x(),aPIm.y(),aDepth)) ; + aResult.AddPair(aPIm,aP); } } return aResult; } /// call variant with vector, depth regularly spaced -cSet2D3D cSensorImage::SyntheticsCorresp3D2D (int aNbByDim,int aNbDepts,double aD0,double aD1) const +cSet2D3D cSensorImage::SyntheticsCorresp3D2D (int aNbByDim,int aNbDepts,double aD0,double aD1,bool IsDepthOrZ) const { std::vector aVDepth; @@ -303,7 +316,7 @@ cSet2D3D cSensorImage::SyntheticsCorresp3D2D (int aNbByDim,int aNbDepts,double aVDepth.push_back(aD0 * pow(aD1/aD0,aW)); } - return SyntheticsCorresp3D2D(aNbByDim,aVDepth); + return SyntheticsCorresp3D2D(aNbByDim,aVDepth,IsDepthOrZ); } bool cSensorImage::IsVisible(const cPt3dr & aP3) const { return DegreeVisibility(aP3) > 0; } diff --git a/MMVII/src/Sensors/cRPC.cpp b/MMVII/src/Sensors/cRPC.cpp index eb7896c1d0..781f55aedc 100755 --- a/MMVII/src/Sensors/cRPC.cpp +++ b/MMVII/src/Sensors/cRPC.cpp @@ -103,6 +103,10 @@ class cDataRPC : public cSensorImage /// Indicate how much a point belongs to sensor visibilty domain double DegreeVisibility(const cPt3dr &) const override; + + bool HasIntervalZ() const override; + cPt2dr GetIntervalZ() const override; + /// const cPixelDomain & PixelDomain() const override; std::string V_PrefixName() const override; @@ -469,6 +473,12 @@ void cDataRPC::Dimap_ReadXMLNorms(const cSerialTree& aTree) mBoxGround = cBox3dr(aP0Gr,aP1Gr); } +bool cDataRPC::HasIntervalZ() const {return true;} +cPt2dr cDataRPC::GetIntervalZ() const +{ + return cPt2dr(mBoxGround.P0().z(),mBoxGround.P1().z()); +} + double cDataRPC::ReadXMLItem(const cSerialTree & aData,const std::string& aPrefix) { return std::stod(aData.GetUniqueDescFromName(aPrefix)->UniqueSon().Value()); @@ -653,6 +663,11 @@ class cAppliTestImportSensors : public cMMVII_Appli cCollecSpecArg2007 & ArgObl(cCollecSpecArg2007 & anArgObl) override ; cCollecSpecArg2007 & ArgOpt(cCollecSpecArg2007 & anArgOpt) override ; + /// Test that the accuracy of ground truth, i.e Proj(P3) = P2 + void TestGroundTruth(const cSensorImage & aSI) const; + /// Test coherence of Direct/Inverse model, i.e Id = Dir o Inv = Inv o Dir + void TestCoherenceDirInv(const cSensorImage & aSI) const; + cPhotogrammetricProject mPhProj; std::string mNameImage; std::string mNameRPC; @@ -673,28 +688,121 @@ cCollecSpecArg2007 & cAppliTestImportSensors::ArgObl(cCollecSpecArg2007 & anArgO return anArgObl << Arg2007(mNameImage,"Name of input Image", {eTA2007::FileDirProj}) << Arg2007(mNameRPC,"Name of input RPC", {eTA2007::Orient}) - << mPhProj.DPPointsMeasures().ArgDirInMand() ; } cCollecSpecArg2007 & cAppliTestImportSensors::ArgOpt(cCollecSpecArg2007 & anArgOpt) { return anArgOpt + << mPhProj.DPPointsMeasures().ArgDirInOpt() << AOpt2007(mShowDetail,"ShowD","Show detail",{eTA2007::HDV}) ; } +void cAppliTestImportSensors::TestGroundTruth(const cSensorImage & aSI) const +{ + // Load mesure from standard MMVII project + cSetMesImGCP aSetMes; + mPhProj.LoadGCP(aSetMes); + mPhProj.LoadIm(aSetMes,mNameImage); + cSet2D3D aSetM23; + aSetMes.ExtractMes1Im(aSetM23,mNameImage); + + cStdStatRes aStCheckIm; // Statistic of reproj errorr + + for (const auto & aPair : aSetM23.Pairs()) // parse all pair to accumulate stat of errors + { + cPt3dr aPGr = aPair.mP3; + cPt2dr aPIm = aSI.Ground2Image(aPGr); + tREAL8 aDifIm = Norm2(aPIm-aPair.mP2); + aStCheckIm.Add(aDifIm); + + if (mShowDetail) + { + StdOut() << "ImGT=" << aDifIm << std::endl; + + } + } + StdOut() << " ============== Accuracy / Ground trurh =============== " << std::endl; + StdOut() << " Avg=" << aStCheckIm.Avg() << ", Worst=" << aStCheckIm.Max() << "\n"; +} + +void cAppliTestImportSensors::TestCoherenceDirInv(const cSensorImage & aSI) const +{ + bool InDepth = ! aSI.HasIntervalZ(); // do we use Im&Depth or Image&Z + + cPt2dr aIntZD = cPt2dr(1,2); + if (InDepth) + { // if depth probably doent matter which one is used + } + else + { + aIntZD = aSI.GetIntervalZ(); // at least with RPC, need to get validity interval + } + + int mNbByDim = 10; + int mNbDepth = 5; + cSet2D3D aS32 = aSI.SyntheticsCorresp3D2D(mNbByDim,mNbDepth,aIntZD.x(),aIntZD.y(),InDepth); + + cStdStatRes aStConsistIm; // stat for image consit Proj( Proj-1(PIm)) ?= PIm + cStdStatRes aStConsistGr; // stat for ground consist Proj-1 (Proj(Ground)) ?= Ground + + for (const auto & aPair : aS32.Pairs()) + { + cPt3dr aPGr = aPair.mP3; + cPt3dr aPIm (aPair.mP2.x(),aPair.mP2.y(),aPGr.z()); + + cPt3dr aPIm2 ; + cPt3dr aPGr2 ; + + if (InDepth) + { + aPIm2 = aSI.Ground2ImageAndDepth(aSI.ImageAndDepth2Ground(aPIm)); + aPGr2 = aSI.ImageAndDepth2Ground(aSI.Ground2ImageAndDepth(aPGr)); + } + else + { + aPIm2 = aSI.Ground2ImageAndZ(aSI.ImageAndZ2Ground(aPIm)); + aPGr2 = aSI.ImageAndZ2Ground(aSI.Ground2ImageAndZ(aPGr)); + } + tREAL8 aDifIm = Norm2(aPIm-aPIm2); + aStConsistIm.Add(aDifIm); + + tREAL8 aDifGr = Norm2(aPGr-aPGr2); + aStConsistGr.Add(aDifGr); + + } + + StdOut() << " ============== Consistencies Direct/Inverse =============== " << std::endl; + StdOut() << " * Image : Avg=" << aStConsistIm.Avg() + << ", Worst=" << aStConsistIm.Max() + << ", Med=" << aStConsistIm.ErrAtProp(0.5) + << std::endl; + + StdOut() << " * Ground: Avg=" << aStConsistGr.Avg() + << ", Worst=" << aStConsistGr.Max() + << ", Med=" << aStConsistGr.ErrAtProp(0.5) + << std::endl; + +} + int cAppliTestImportSensors::Exe() { mPhProj.FinishInit(); + cDataRPC aDataRPC(mNameRPC,mNameImage); + + if (mPhProj.DPPointsMeasures().DirInIsInit()) + TestGroundTruth(aDataRPC); + + TestCoherenceDirInv(aDataRPC); + /* cSetMesImGCP aSetMes; mPhProj.LoadGCP(aSetMes); mPhProj.LoadIm(aSetMes,mNameImage); - cDataRPC aDataRPC(mNameRPC,mNameImage); cSet2D3D aSetM23; aSetMes.ExtractMes1Im(aSetM23,mNameImage); @@ -702,20 +810,21 @@ int cAppliTestImportSensors::Exe() tREAL8 aSomCheckIm = 0.0; tREAL8 aSomConsistIm = 0.0; tREAL8 aSomConsistGr = 0.0; + const cSensorImage & aSI (aDataRPC); for (const auto & aPair : aSetM23.Pairs()) { cPt3dr aPGr = aPair.mP3; tREAL8 aZ = aPGr.z(); - cPt2dr aPIm = aDataRPC.Ground2Image(aPGr); + cPt2dr aPIm = aSI.Ground2Image(aPGr); // @G2I @I2G @G2I // Pgr=mP3 ---> PIm ---> aPGr2?=Pgr ---> aPIm2 // ?=mP2 ?=Pgr ?= PIm // "GTTest" "GrCons" "GrCons" - cPt3dr aPGr2 = aDataRPC.ImageZToGround(aPIm,aZ); - cPt2dr aPIm2 = aDataRPC.Ground2Image(aPGr2); + cPt3dr aPGr2 = aSI.ImageAndZ2Ground(cPt3dr(aPIm.x(),aPIm.y(),aZ)); + cPt2dr aPIm2 = aSI.Ground2Image(aPGr2); aSomCheckIm += Norm2(aPIm - aPair.mP2); aSomConsistIm += Norm2(aPIm-aPIm2); @@ -738,6 +847,7 @@ int cAppliTestImportSensors::Exe() StdOut() << " CheckGTIm=" << aSomCheckIm << "\n"; StdOut() << " ConsistIm=" << aSomConsistIm << "\n"; StdOut() << " ConsistGr=" << aSomConsistGr << "\n"; + */ return EXIT_SUCCESS; } diff --git a/MMVII/src/Sensors/cSensorCamPC.cpp b/MMVII/src/Sensors/cSensorCamPC.cpp index e689106622..f52904528a 100644 --- a/MMVII/src/Sensors/cSensorCamPC.cpp +++ b/MMVII/src/Sensors/cSensorCamPC.cpp @@ -213,6 +213,9 @@ double cSensorCamPC::DegreeVisibilityOnImFrame(const cPt2dr & aP) const return mInternalCalib->DegreeVisibilityOnImFrame(aP); } +bool cSensorCamPC::HasImageAndDepth() const {return true;} + + cPt3dr cSensorCamPC::Ground2ImageAndDepth(const cPt3dr & aP) const { cPt3dr aPCam = Pose().Inverse(aP); // P in camera coordinate @@ -524,7 +527,7 @@ void cSensorCamPC::GetAdrInfoParam(cGetAdrInfoParam & aGAIP) void cSensorCamPC::Bench() { - cSet2D3D aSet32 = SyntheticsCorresp3D2D(20,3,1.0,10.0) ; + cSet2D3D aSet32 = SyntheticsCorresp3D2D(20,3,1.0,10.0,true) ; tREAL8 aRes = AvgAngularProjResiudal(aSet32); MMVII_INTERNAL_ASSERT_bench(aRes<1e-8,"Avg res ang");