SLAM.LoopClosure.RGBDVisualVocabulary.Learn

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

function Learn
    input State previous;
    input Real descriptor[featureCapacity,descriptorSize];
    input Real descriptorEnabled[featureCapacity];
    input Real descriptorCount "Full domain extent, including disabled sparse slots";
    input Integer generation;
    input Integer sourceRevision;
    input Integer version;
    input Boolean requested = true;
    input Integer minimumMeasuredDescriptors = 8;
    input Real minimumDistanceSquared = 0.04;
    output State next;
    output Boolean accepted;
    output Integer reason "0 idle, 1 frozen now, 2 learning, 3 already frozen; negative refusal";
  protected
    State pending;
    Real samples[featureCapacity,descriptorSize];
    Real sample[descriptorSize]; Boolean sampleValid; Boolean inputsValid;
    Integer domainCount; Integer measuredCount;
    Real distance; Real nearest;
  algorithm
    next := previous; accepted := false; reason := 0;
    pending := previous; samples := zeros(featureCapacity,descriptorSize);
    sample := zeros(descriptorSize); sampleValid := false; inputsValid := true;
    domainCount := 0; measuredCount := 0; distance := 0.0; nearest := 5.0;
    if requested then
      if not Valid(previous) then
        reason := -1;
      elseif generation < 1 or generation > identifierLimit or sourceRevision < 1
        or sourceRevision > identifierLimit or version < 1 or version > identifierLimit
        or generation <> previous.generation or sourceRevision <> previous.sourceRevision
        or version <> previous.version then
        reason := -2;
      elseif minimumMeasuredDescriptors < 1 or minimumMeasuredDescriptors > featureCapacity
        or not (minimumDistanceSquared >= 1e-12 and minimumDistanceSquared <= 4.0) then
        reason := -3;
      elseif not (descriptorCount >= 0.0 and descriptorCount <= featureCapacity
        and floor(descriptorCount) == descriptorCount) then
        reason := -4;
      else
        domainCount := integer(descriptorCount);
        // Complete preflight before adding even the first word. No enabled slot
        // outside the supplied domain, fractional mask or active poison can commit.
        for feature in 1:featureCapacity loop
          inputsValid := inputsValid and (descriptorEnabled[feature] == 0.0
            or descriptorEnabled[feature] == 1.0)
            and (feature <= domainCount or descriptorEnabled[feature] == 0.0);
          if feature <= domainCount and descriptorEnabled[feature] == 1.0 then
            (sample,sampleValid) := NormalizeVisualWord(descriptor[feature,:],1.0);
            inputsValid := inputsValid and sampleValid;
            samples[feature,:] := sample;
            measuredCount := measuredCount+1;
          end if;
        end for;
        if not inputsValid then
          reason := -4;
        elseif previous.ready then
          accepted := true; reason := 3;
        else
          // Fixed-bounded nearest representative admission, source raster order.
          // Identical samples count as measurements but do not consume more words.
          for feature in 1:featureCapacity loop
            if feature <= domainCount and descriptorEnabled[feature] == 1.0
              and pending.count < vocabularyCapacity then
              nearest := 5.0;
              for word in 1:vocabularyCapacity loop
                if word <= pending.count then
                  distance := 0.0;
                  for component in 1:descriptorSize loop
                    distance := distance+(samples[feature,component]-pending.words[word,component])
                      *(samples[feature,component]-pending.words[word,component]);
                  end for;
                  nearest := min(nearest,distance);
                end if;
              end for;
              if pending.count == 0 or nearest >= minimumDistanceSquared then
                pending.count := pending.count+1;
                pending.words[pending.count,:] := samples[feature,:];
                pending.enabled[pending.count] := 1.0;
              end if;
            end if;
          end for;
          pending.ready := measuredCount >= minimumMeasuredDescriptors and pending.count >= 1;
          next := pending; accepted := true; reason := if pending.ready then 1 else 2;
        end if;
      end if;
    end if;
  end Learn;