Commit 8197fbce authored by Nafis A Abeer's avatar Nafis A Abeer Committed by Rohan Kumar
Browse files

updates

parent d38ff772
Loading
Loading
Loading
Loading
+10 KiB

File added.

No diff preview for this file type.

+611 KiB

File added.

No diff preview for this file type.

+49.9 KiB

File added.

No diff preview for this file type.

+94 −0
Original line number Diff line number Diff line
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
+54 −0
Original line number Diff line number Diff line
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
Loading