SLAM.PoseGraph.RGBDGraphMeasurements.ValidProposal

Parameters, inputs, outputs, defaults and equations from the source file.

function ValidProposal
    input RGBDKeyframes.Catalog catalog;
    input RGBDLoopVerification.Proposal proposal;
    input Real inlierDistance = 0.08;
    input Real maximumRms = 0.03;
    output Boolean valid;
    input Boolean calibratedResiduals = true;
    input Real maximumNormalizedSquared = 9.0;
    input Real localizationSigma = 0.5; input Real depthInflation = 1.0; input Real minimumPivot = 1e-10;
  protected
    Integer referenceSlot; Integer currentSlot; Integer partner; Integer inliers;
    Boolean used[RGBDKeyframes.featureCapacity]; Boolean pairValid;
    Real A[dimension,dimension]; Real D[dimension,dimension]; Real u[dimension];
    Real residual[dimension]; Real cost;
    Real referencePoint[dimension]; Real currentPoint[dimension];
    Real referenceCovariance[dimension,dimension]; Real currentCovariance[dimension,dimension];
    Real squared; Boolean residualValid;
  algorithm
    referenceSlot := 1; currentSlot := 1; partner := 1; inliers := 0; cost := 0.0;
    used := fill(false,RGBDKeyframes.featureCapacity);
    valid := RGBDKeyframes.ValidHeader(catalog) and proposal.verified and proposal.rejectionReason == 0
      and proposal.generation == catalog.generation
      and proposal.referenceId >= 1 and proposal.referenceId < proposal.currentId
      and proposal.currentId < catalog.nextId
      and proposal.matchedCount >= proposal.inlierCount
      and proposal.matchedCount <= RGBDKeyframes.featureCapacity
      and proposal.inlierCount >= 3 and proposal.inlierCount <= RGBDKeyframes.featureCapacity
      and proposal.rms >= 0 and proposal.rms <= (if calibratedResiduals then 1e6 else maximumRms)
      and inlierDistance > 0 and inlierDistance <= 10
      and maximumRms >= 0 and maximumRms <= inlierDistance
      and RGBDUncertaintyProper(proposal.opticalRotation)
      and RGBDUncertaintyProper(proposal.bodyRotation)
      and ValidUncertainty(proposal.covariance,proposal.information);
    if calibratedResiduals then
      valid := valid and maximumNormalizedSquared > 0 and maximumNormalizedSquared <= 1e6
        and localizationSigma > 0 and localizationSigma <= 10
        and depthInflation >= 1 and depthInflation <= 100 and minimumPivot > 0 and minimumPivot < 1;
    end if;
    if valid then
      valid := proposal.currentId == catalog.nextId-1;
      referenceSlot := mod(proposal.referenceId-1,nodeCapacity)+1;
      currentSlot := mod(proposal.currentId-1,nodeCapacity)+1;
      valid := Bound(catalog,proposal.referenceId,referenceSlot,proposal.referenceEpoch)
        and Bound(catalog,proposal.currentId,currentSlot,proposal.currentEpoch);
      if valid and calibratedResiduals then
        // Admission owns its policy and recomputes every residual from the
        // retained calibrated points. A verifier's Boolean is not sufficient.
        valid := catalog.disparityNoises[referenceSlot] > 0 and catalog.disparityNoises[referenceSlot] <= 10
          and catalog.disparityNoises[referenceSlot] == catalog.disparityNoises[currentSlot]
          and catalog.baselines[referenceSlot] > 0 and catalog.baselines[referenceSlot] <= 1
          and catalog.baselines[referenceSlot] == catalog.baselines[currentSlot]
          and catalog.noiseReferenceFocals[referenceSlot] > 0 and catalog.noiseReferenceFocals[referenceSlot] <= 1e6
          and catalog.noiseReferenceFocals[currentSlot] > 0 and catalog.noiseReferenceFocals[currentSlot] <= 1e6;
        for axis in 1:2 loop
          valid := valid and catalog.rgbCalibrations[referenceSlot,axis] > 0 and catalog.rgbCalibrations[referenceSlot,axis] <= 1e6
            and catalog.rgbCalibrations[currentSlot,axis] > 0 and catalog.rgbCalibrations[currentSlot,axis] <= 1e6;
        end for;
      end if;
      if valid then
        A := catalog.opticalToBodyRotations[referenceSlot,:,:]*transpose(proposal.opticalRotation);
        D := A*transpose(catalog.opticalToBodyRotations[currentSlot,:,:]);
        u := catalog.cameraOriginsBody[referenceSlot,:]-A*(proposal.opticalTranslation
          +transpose(catalog.opticalToBodyRotations[currentSlot,:,:])*catalog.cameraOriginsBody[currentSlot,:]);
        valid := max(abs(D-proposal.bodyRotation)) <= 1e-7
          and max(abs(u-proposal.bodyTranslation)) <= 1e-7;
        for axis in 1:dimension loop
          valid := valid and abs(proposal.opticalTranslation[axis]) <= 1e6
            and abs(proposal.bodyTranslation[axis]) <= 1e6;
        end for;
        for feature in 1:RGBDKeyframes.featureCapacity loop
          if proposal.inliers[feature] then
            inliers := inliers+1;
            partner := proposal.partners[feature];
            pairValid := catalog.featureEnabled[referenceSlot,feature]
              and partner >= 1 and partner <= RGBDKeyframes.featureCapacity;
            if pairValid then
              pairValid := catalog.featureEnabled[currentSlot,partner] and not used[partner];
              used[partner] := true;
              referencePoint := catalog.opticalPoints[referenceSlot,feature,:];
              currentPoint := catalog.opticalPoints[currentSlot,partner,:];
              residual := proposal.opticalRotation*referencePoint+proposal.opticalTranslation-currentPoint;
              if calibratedResiduals then
                pairValid := pairValid and referencePoint[3] > 0 and currentPoint[3] > 0
                  and max(abs(referencePoint)) <= 1e6 and max(abs(currentPoint)) <= 1e6;
                if pairValid then
                  referenceCovariance := RGBDOpticalPointCovariance(referencePoint,
                    catalog.rgbCalibrations[referenceSlot,1],catalog.rgbCalibrations[referenceSlot,2],localizationSigma,
                    catalog.disparityNoises[referenceSlot],catalog.noiseReferenceFocals[referenceSlot],catalog.baselines[referenceSlot],depthInflation);
                  currentCovariance := RGBDOpticalPointCovariance(currentPoint,
                    catalog.rgbCalibrations[currentSlot,1],catalog.rgbCalibrations[currentSlot,2],localizationSigma,
                    catalog.disparityNoises[currentSlot],catalog.noiseReferenceFocals[currentSlot],catalog.baselines[currentSlot],depthInflation);
                  (residualValid,squared) := RegistrationPairResidual(referencePoint,currentPoint,
                    proposal.opticalRotation,proposal.opticalTranslation,true,referenceCovariance,currentCovariance,minimumPivot);
                  pairValid := residualValid and squared <= maximumNormalizedSquared;
                end if;
              else
                pairValid := pairValid and residual*residual <= inlierDistance^2;
              end if;
              cost := cost+residual*residual;
            end if;
            valid := valid and pairValid;
          else
            valid := valid and proposal.partners[feature] == 0;
          end if;
        end for;
        valid := valid and inliers == proposal.inlierCount;
        if inliers > 0 then valid := valid and abs(sqrt(cost/inliers)-proposal.rms) <= 1e-6; end if;
      end if;
    end if;
  end ValidProposal;