updates

function [isLoopClosed, mapPoints, vSetKeyFrames] = helperAddLoopConnections(...

    mapPoints, vSetKeyFrames, loopCandidates, currKeyFrameId, currFeatures, ...

    loopEdgeNumMatches)

%helperAddLoopConnections add connections between the current key frame and

%   the valid loop candidate key frames. A loop candidate is valid if it has

%   enough covisible map points with the current key frame.

%   This is an example helper function that is subject to change or removal

%   in future releases.

%   Copyright 2019-2023 The MathWorks, Inc.

%#codegen

loopClosureEdge = zeros(0, 2, 'uint32');

numCandidates   = size(loopCandidates,1);

if isSimMode

    [index3d1, index2d1] = findWorldPointsInView(mapPoints, currKeyFrameId);

else

    [index3d1Cg, index2d1Cg] = findWorldPointsInView(mapPoints, currKeyFrameId);

    index2d1 = index2d1Cg{1};

    index3d1 = index3d1Cg{1};

end

validFeatures1  = currFeatures.Features(index2d1, :);

for k = 1 : numCandidates

    if isSimMode()

        [index3d2, index2d2] = findWorldPointsInView(mapPoints, loopCandidates(k));

    else

        [index3d2Cg, index2d2Cg] = findWorldPointsInView(mapPoints, loopCandidates(k));

        index2d2 = index2d2Cg{1};

        index3d2 = index3d2Cg{1};

    end

    allFeatures2   = vSetKeyFrames.Views.Features{loopCandidates(k)};

    validFeatures2 = allFeatures2(index2d2, :);

    indexPairs = matchFeatures(binaryFeatures(validFeatures1), binaryFeatures(validFeatures2), ...

        'Unique', true, 'MaxRatio', 0.9, 'MatchThreshold', 40);

    % Check if all the candidate key frames have strong connection with the

    % current keyframe

    if size(indexPairs, 1) < loopEdgeNumMatches

        continue

    end

    % Estimate the relative pose of the current key frame with respect to the

    % loop candidate keyframe with the highest similarity score

    worldPoints1 = mapPoints.WorldPoints(index3d1(indexPairs(:, 1)), :);

    worldPoints2 = mapPoints.WorldPoints(index3d2(indexPairs(:, 2)), :);

    tform1 = pose2extr(vSetKeyFrames.Views.AbsolutePose(end));

    tform2 = pose2extr(vSetKeyFrames.Views.AbsolutePose(loopCandidates(k)));

    worldPoints1InCamera1 = transformPointsForward(tform1, worldPoints1) ;

    worldPoints2InCamera2 = transformPointsForward(tform2, worldPoints2) ;

    w = warning('off','all');

    if isSimMode()

        [tform, inlierIndex] = estgeotform3d(...

            worldPoints1InCamera1, worldPoints2InCamera2, 'similarity', 'MaxDistance', 0.1);

    else

        [tform, inlierIndex] = estgeotform3d(...

            worldPoints1InCamera1, worldPoints2InCamera2, 'rigid', 'MaxDistance', 0.1);

    end

    warning(w);

    % Add connection between the current key frame and the loop key frame

    inlierIndexVals = inlierIndex(:);

    indexPairs1 = indexPairs(inlierIndexVals, 1);

    indexPairs2 = indexPairs(inlierIndexVals, 2);

    index2dPairs = index2d2(indexPairs2);

    index2d1Pairs = index2d1(indexPairs1);

    matches = uint32([index2dPairs, index2d1Pairs]);

    vSetKeyFrames = addConnection(vSetKeyFrames, loopCandidates(k), currKeyFrameId, tform, 'Matches', matches);

    if isSimMode()

        disp(['Loop edge added between keyframe: ', num2str(loopCandidates(k)), ' and ', num2str(currKeyFrameId)]);

    end

    % Fuse co-visible map points

    matchedIndex3d1 = index3d1(indexPairs1);

    matchedIndex3d2 = index3d2(indexPairs2);

    mapPoints = updateWorldPoints(mapPoints, matchedIndex3d1, mapPoints.WorldPoints(matchedIndex3d2, :));

    loopClosureEdge = [loopClosureEdge; loopCandidates(k), currKeyFrameId];

end

isLoopClosed = ~isempty(loopClosureEdge);

end

function tf = isSimMode()

tf = isempty(coder.target);

end

 No newline at end of file

function [mapPoints, vSetKeyFrames] = helperAddNewKeyFrame(mapPoints, vSetKeyFrames,...

    cameraPose, currFeatures, currPoints, mapPointsIndices, featureIndices, keyFramesIndices)

%helperAddNewKeyFrame add key frames to the key frame set

%

%   This is an example helper function that is subject to change or removal

%   in future releases.

%   Copyright 2019-2023 The MathWorks, Inc.

%#codegen

viewId = vSetKeyFrames.Views.ViewId(end)+1;

vSetKeyFrames = addView(vSetKeyFrames, viewId, cameraPose,...

    'Features', currFeatures.Features, ...

    'Points', currPoints);

viewsAbsPoses = vSetKeyFrames.Views.AbsolutePose;

for i = 1:numel(keyFramesIndices)

    localKeyFrameId = keyFramesIndices(i);

    if isSimMode()

        [index3d, index2d] = findWorldPointsInView(mapPoints, localKeyFrameId);

    else

        [index3dCg, index2dCg] = findWorldPointsInView(mapPoints, localKeyFrameId);

        index3d = index3dCg{1};

        index2d = index2dCg{1};

    end

    [~, ia, ib] = intersect(index3d, mapPointsIndices, 'stable');

    prePose   = viewsAbsPoses(localKeyFrameId);

    relPose = rigidtform3d(prePose.R' * cameraPose.R, ...

        (cameraPose.Translation-prePose.Translation)*prePose.R);

    if numel(ia) > 5

        if isSimMode()

            vSetKeyFrames = addConnection(vSetKeyFrames, localKeyFrameId, viewId, relPose, ...

                'Matches', [index2d(ia),featureIndices(ib)]);

        else

            coder.varsize('matches', [inf 2], [1, 0]);

            fIndices = featureIndices(ib(:));

            matches = [index2d(ia), fIndices];

            vSetKeyFrames = addConnection(vSetKeyFrames, localKeyFrameId, viewId, relPose, ...

                'Matches', matches);

        end

    end

end

mapPoints = addCorrespondences(mapPoints, viewId, mapPointsIndices, ...

    featureIndices);

end

function tf = isSimMode()

    tf = isempty(coder.target);

end

 No newline at end of file