Map Large Environments Using Multi-Agent Collaborative SLAM

R2026b

Since R2026b

This example uses:

This example shows how to map a large environment using multiple stereo-camera agents that independently run visual SLAM, detect shared locations via visual place recognition, and merge their maps through pose graph optimization.

A single robot mapping a large environment is constrained by battery life, travel speed, and accumulated drift. By deploying multiple agents that explore different regions simultaneously, you can cover more area in less time. The key challenge is determining when two agents have visited the same physical location, known as a crossing point, and using that information to align their independently built maps into a unified global map.

The collaborative SLAM workflow involves four stages:

Independent exploration — Each agent runs stereo visual SLAM to build its own local map using separate stereovslam objects.
Visual place recognition — At regular intervals, agents compare keyframe descriptors against each other's DBoW2 databases using dbowLoopDetector to detect crossing points.
Map merging — When a crossing is geometrically verified, the relative transform between agent coordinate frames is computed, aligning them into a common reference frame.
Pose graph optimization — Once all agents are connected, a factorGraph (Navigation Toolbox) with odometry and crossing constraints is optimized to produce globally consistent trajectories.

Environment and Agent Setup

This example uses pre-captured stereo images from an Unreal Engine scene showing a city block. Three UAV agents equipped with stereo cameras traverse different routes through the scene. Each agent captures left-right image pairs along its path. The agents' routes partially overlap. These overlapping regions are where crossing points will be detected and used to merge the individual maps.

Configure Multi-Agent SLAM System

Define the dataset paths, camera parameters, and algorithm settings for the multi-agent system. A single config struct stores all parameters, making it easy to tune the system or adapt it to a different dataset.

Download the stereo image dataset for three agents and specify the shared visual vocabulary file used for crossing detection.

% For reproducibility
rng(42);
config.agentFolders = helperDownloadData();

Downloading multi-agent stereo dataset (agents.tar)...

config.vocabFile = ".\customVocab.bin";

Define camera intrinsics and stereo baseline. These describe the stereo geometry used for depth estimation via semi-global matching (SGM).

config.focalLength     = [1109, 1109];
config.principalPoint  = [640, 360];
config.imageSize       = [720, 1280];
config.baseline        = 0.5;

Configure stereovslam parameters that control keyframe creation, feature tracking, and local loop closure. Lower skipMaxFrames produces denser keyframes with less drift; trackFeatureRange defines when new keyframes are triggered based on tracked feature count.

config.disparityRange       = [0, 64];
config.loopClosureThreshold = 120;
config.maxNumPoints         = 800;
config.skipMaxFrames        = 10;
config.trackFeatureRange    = [30, 120];

Configure crossing detection parameters.

crossingCheckInterval sets how often (in keyframes) agents query each other's DBoW2 databases. When agents are near a potential crossing, the interval narrows to one-third for faster detection.
minDBoWSimilarity is the minimum visual similarity score when comparing one agent's keyframe against another agent's database. Higher values require stronger visual overlap to trigger a crossing candidate.
maxCrossingError specifies the maximum allowable [rotationDeg, translationM] for a crossing candidate to be accepted.

config.crossingCheckInterval      = 10;
config.crossingCheckIntervalFine  = round(config.crossingCheckInterval/3);
config.minDBoWSimilarity          = 0.15;
config.maxCrossingError           = [15, 2];

Load Stereo Data and Visual Vocabulary

Load pre-recorded stereo image sequences for three agents exploring different regions of an indoor environment. Each agent carries a stereo camera and captures left-right image pairs as it moves.

The helperLoadAgentData function:

Creates image datastores for left and right images of each agent
Constructs camera intrinsics from the configuration

numAgents = numel(config.agentFolders);
[imdsLeftAgent, imdsRightAgent, agentNumFrames, intrinsics] = helperLoadAgentData(config);

Compute the stereo reprojection matrix used later to reconstruct 3-D points during crossing verification.

extrinsicsCam2 = rigidtform3d(eye(3), [-config.baseline, 0, 0]);
stereoParams = stereoParameters(intrinsics, intrinsics, extrinsicsCam2);
[~, ~, reprojectionMatrix] = rectifyStereoImages(...
    readimage(imdsLeftAgent{1}, 1), readimage(imdsRightAgent{1}, 1), stereoParams);

Load a shared visual vocabulary for cross-agent place recognition. All agents use the same vocabulary so that their DBoW2 descriptors are directly comparable.

customBag = bagOfFeaturesDBoW(config.vocabFile);

Create SLAM Agents and Crossing Detectors

Each agent needs two capabilities: building its own local map from stereo frames, and recognizing places that other agents have already visited. Together, the SLAM objects and crossing detectors enable each agent to explore independently and trigger a map merge when one agent visits a region already seen by another. Create these per-agent artifacts:

vslamAgents — One stereovslam object instance per agent. Each builds its own local map using stereo visual odometry with ORB features and SGM depth estimation.
crossingDetectors — One dbowLoopDetector per agent backed by the shared visual vocabulary. Other agents query this database to find visually similar keyframes.
keyframeLookup — Maps each indexed keyframe back to its original dataset frame number, which is used to retrieve images during crossing verification.
numKeyFrames — Running keyframe count per agent, used to schedule crossing checks at regular intervals.

vslamAgents  = cell(1, numAgents);
crossingDetectors = cell(1, numAgents);
keyframeLookup   = cell(1, numAgents);
numKeyFrames  = zeros(1, numAgents);
for agentIdx = 1:numAgents
    vslamAgents{agentIdx} = stereovslam(intrinsics, config.baseline, ...
        DisparityRange=config.disparityRange, ...
        LoopClosureThreshold=config.loopClosureThreshold, ...
        MaxNumPoints=config.maxNumPoints, ...
        SkipMaxFrames=config.skipMaxFrames, ...
        TrackFeatureRange=config.trackFeatureRange, ...
        ThreadLevel=1);
    crossingDetectors{agentIdx} = dbowLoopDetector(customBag);
    keyframeLookup{agentIdx} = zeros(0, 2);
end

Set Up Map Merging and Visualization

Agent 1 serves as the global reference frame. As crossings are detected, other agents are progressively aligned into this frame via rigid transforms. Create the state variables that track merge progress throughout the main loop:

isMerged(k) — Whether agent k has been aligned with Agent 1.
agent2WorldTransforms{k} — The rigid transform that maps agent k's local coordinates into Agent 1's world frame.

isMerged = false(1, numAgents);
isMerged(1) = true;
agent2WorldTransforms = cell(1, numAgents);
agent2WorldTransforms{1} = rigidtform3d;

Crossing detection is not instantaneous. Agents may approach a previously visited region over several keyframes before a strong visual match is confirmed. Create the variables that manage adaptive scheduling of crossing checks:

crossings — Accumulates verified crossing structs, each storing the pair of agent and keyframe indices involved and the relative camera transform between them. This transform is later used as a measurement constraint in pose graph optimization.
isNearCrossing(a,b) — Flags when agents a and b are close to a potential crossing, triggering more frequent DBoW2 queries.
lastCrossingCheckKeyFrames(a,b) — Records the last keyframe index at which pair (a,b) was checked, throttling redundant queries.

allCrossings = {};
isNearCrossing = false(numAgents, numAgents);
lastCrossingCheckKeyFrames = zeros(numAgents, numAgents);

Once all agents are connected through crossings, pose graph optimization produces globally consistent trajectories. Create variables to store the optimization output:

optimizedPoses — Stores the pose-graph-optimized trajectories.
pgoHasRun — One-shot flag that, after the first optimization, switches per-agent trajectory lines to dashed to distinguish them from the optimized result and enables display of the merged point cloud.

optimizedPoses = cell(1, numAgents);
pgoHasRun = false;

Create visualizers to display agent trajectories with the merged 3-D map, and to show stereo image pairs as each agent explores.

agentColors = [0 0 1; 1 0 0; 1 0 1];
viz = helperCollabSlamVisualizer(numAgents, agentColors);
[figImgs, axImgs, hImgs] = helperSetupStereoDisplay(config, numAgents);

Run Collaborative Multi-Agent SLAM Loop

The main loop drives the collaborative SLAM workflow. Each agent explores independently, but at every new keyframe the system checks whether any two agents have visited the same place. When a crossing is confirmed, the agents' maps are merged. Once all agents are connected, the system performs global pose graph optimization. The loop performs these steps per iteration:

Add stereo frames — Feed left-right image pairs into each agent's stereovslam object.
- Estimates motion and creates keyframes when sufficient parallax is detected.
Extract cross-agent features — At regular keyframe intervals, extract ORB descriptors and add them to the agent's DBoW2 database.
- Makes each agent's keyframes discoverable by other agents.
Detect crossings — Query other agents' DBoW2 databases for visually similar keyframes.
- When agents are near a potential crossing zone, an adaptive check interval increases query frequency.
Verify crossings geometrically — For each candidate, compute the metric-scale relative pose between stereo pairs.
- Use SGM disparity, 3-D reconstruction, and RANSAC registration.
- Rejects candidates where rotation or translation exceeds maxCrossingError.
Merge maps — When a verified crossing satisfies both thresholds, compute the transform chain aligning the unmerged agent's world frame to Agent 1's frame.
- Propagates merges transitively. For example, if Agent 2 merges with Agent 1 and a prior crossing linked Agent 3 to Agent 2, Agent 3 can now be merged through the chain.
Optimize pose graph — Once all agents are connected, build a factor graph and optimize for globally consistent trajectories
- Nodes — keyframe poses from each agent
- Intra-agent edges — odometry constraints from stereovslam
- Inter-agent edges — crossing constraints from geometric verification.
- Output — Optimized poses in a unified coordinate frame.

% Set iteration count so every agent processes all its frames
numFrames = max(agentNumFrames);
for frameIdx = 1:numFrames
    anyNewKeyFrame = false;
    for agentIdx = 1:numAgents
        if frameIdx > agentNumFrames(agentIdx)
            continue 
        end

        % Read stereo pair and feed to VSLAM
        imgL = readimage(imdsLeftAgent{agentIdx}, frameIdx);
        imgR = readimage(imdsRightAgent{agentIdx}, frameIdx);
        if mod(frameIdx, 5) == 0
            hImgs(agentIdx).CData = [imgL, imgR];
            title(axImgs(agentIdx), sprintf("Agent %d — Frame %d/%d", agentIdx, frameIdx, agentNumFrames(agentIdx)));
        end
        addFrame(vslamAgents{agentIdx}, imgL, imgR);

        % On new keyframe: check for crossings against other agents
        if hasNewKeyFrame(vslamAgents{agentIdx})
            anyNewKeyFrame = true;
            numKeyFrames(agentIdx) = numKeyFrames(agentIdx) + 1;

            % Extract ORB features and add to this agent's DBoW2 database
            % at regular intervals
            featuresReady = false;
            currFeatures = [];
            if mod(numKeyFrames(agentIdx), config.crossingCheckInterval) == 0
                currFeatures = helperExtractKeyframeORBFeatures(imgL);
                addVisualFeatures(crossingDetectors{agentIdx}, numKeyFrames(agentIdx), currFeatures);
                keyframeLookup{agentIdx}(end+1, :) = [numKeyFrames(agentIdx), frameIdx];
                featuresReady = true;
            end

            % Cross-agent crossing detection
            for b = 1:numAgents
                if b == agentIdx || size(keyframeLookup{b}, 1) == 0
                    continue
                end

                % Adaptive check interval: finer when agents are nearby
                if isNearCrossing(agentIdx, b)
                    checkInterval = config.crossingCheckIntervalFine;
                else
                    checkInterval = config.crossingCheckInterval;
                end
                if numKeyFrames(agentIdx) - lastCrossingCheckKeyFrames(agentIdx, b) < checkInterval
                    continue
                end

                % Extract features on demand if not yet done this keyframe
                if ~featuresReady
                    currFeatures = helperExtractKeyframeORBFeatures(imgL);
                    addVisualFeatures(crossingDetectors{agentIdx}, numKeyFrames(agentIdx), currFeatures);
                    keyframeLookup{agentIdx}(end+1, :) = [numKeyFrames(agentIdx), frameIdx];
                    featuresReady = true;
                end

                % Query agent b's DBoW2 database for visual matches
                lastCrossingCheckKeyFrames(agentIdx, b) = numKeyFrames(agentIdx);
                [hasVisualMatch, candidateKeyFrameIds] = helperCheckCrossingDBoW2(...
                    crossingDetectors{b}, currFeatures, keyframeLookup{b}(:,1), config.minDBoWSimilarity);
                if ~hasVisualMatch
                    isNearCrossing(agentIdx, b) = false;
                    continue
                end

                % Iterate over crossing candidates and verify geometrically
                crossingAccepted = false;
                agentsNearby = false;
                for candIdx = 1:numel(candidateKeyFrameIds)
                    lookupIdx = find(keyframeLookup{b}(:,1) == candidateKeyFrameIds(candIdx), 1);
                    if isempty(lookupIdx)
                        continue
                    end
                    [crossingCandidate, isVerified] = helperVerifyCrossing(agentIdx, b, numKeyFrames, ...
                        keyframeLookup, lookupIdx, imgL, imgR, imdsLeftAgent, imdsRightAgent, ...
                        vslamAgents, reprojectionMatrix, config);

                    % Accept crossing if translation is within threshold
                    if isVerified
                        translationDist = norm(crossingCandidate.relativeCameraPose.Translation);
                        if translationDist > config.maxCrossingError(2)
                            agentsNearby = true;
                        else
                            allCrossings{end+1} = crossingCandidate;
                            prevMerged = isMerged;
                            [isMerged, agent2WorldTransforms] = helperTryMergeAgents(...
                                crossingCandidate, allCrossings, isMerged, agent2WorldTransforms, vslamAgents);
                            helperLogMergeEvent(prevMerged, isMerged, crossingCandidate, translationDist, length(allCrossings));

                            % Run PGO once all agents are connected
                            if all(isMerged) && length(allCrossings) >= 2
                                optimizedPoses = helperOptimizeMultiAgentTrajectories(...
                                    vslamAgents, allCrossings, agent2WorldTransforms, isMerged);
                                if ~pgoHasRun
                                    viz.setDashedTrajectories();
                                    pgoHasRun = true;
                                end
                            end
                            crossingAccepted = true;
                        end
                    end
                    if crossingAccepted
                        break
                    end
                end

                % Update adaptive check interval state.
                % If neither accepted nor nearby, keep current state unchanged.
                if crossingAccepted
                    isNearCrossing(agentIdx, b) = false;
                elseif agentsNearby
                    isNearCrossing(agentIdx, b) = true;
                end
            end
        end
    end

Refresh the stereo image panels and 3-D trajectory plot. When at least two agents are merged, the combined point cloud is updated at a frequency that increases once the first cloud is rendered.

    % Update stereo image panels and 3-D trajectory plot
    if mod(frameIdx, 5) == 0
        drawnow limitrate
    end
    if anyNewKeyFrame || mod(frameIdx, 50) == 0
        updatePlot(viz,vslamAgents, agent2WorldTransforms, isMerged, optimizedPoses, pgoHasRun);
        if viz.hasPointCloud()
            pcUpdateFrequency = 250;
        else
            pcUpdateFrequency = 50;
        end
        if sum(isMerged) >= 2 && mod(frameIdx, pcUpdateFrequency) == 0
            updatePointCloud(viz,vslamAgents, agent2WorldTransforms);
        end
    end
end

[Merge] Agent 3 merged into the global frame via crossing between Agent 1 and Agent 3 (distance: 0.48 m, total crossings: 1)
[Merge] Agent 2 merged into the global frame via crossing between Agent 1 and Agent 2 (distance: 1.51 m, total crossings: 3)
[Refine] New crossing between Agent 2 and Agent 3 adds merge constraint (crossing #4)
[Refine] New crossing between Agent 1 and Agent 2 adds merge constraint (crossing #5)
[Refine] New crossing between Agent 1 and Agent 2 adds merge constraint (crossing #6)
[Refine] New crossing between Agent 2 and Agent 3 adds merge constraint (crossing #7)
[Refine] New crossing between Agent 1 and Agent 2 adds merge constraint (crossing #8)
[Refine] New crossing between Agent 2 and Agent 3 adds merge constraint (crossing #9)
[Refine] New crossing between Agent 1 and Agent 2 adds merge constraint (crossing #10)

Visualize Merged Map and Trajectories

The final visualization shows all three agent trajectories in a common coordinate frame. Dashed lines represent the original odometry-only trajectories, while the solid green line shows the pose-graph-optimized result. The 3-D point cloud is colored by elevation, revealing the structure of the mapped environment.

The helperCollabSlamVisualizer class:

Displays per-agent trajectory lines with camera frustums using pcplayer (Point Cloud Toolbox) and plotCamera
Overlays the merged pose-graph-optimized trajectory
Streams the combined 3-D point cloud with elevation-based or per-agent coloring
Auto-scales axes as data arrives

updatePlot(viz,vslamAgents, agent2WorldTransforms, isMerged, optimizedPoses, pgoHasRun);
updatePointCloud(viz,vslamAgents, agent2WorldTransforms);

Figure Point Cloud Player contains an axes object. The axes object with title Trajectories & Point Cloud, xlabel X (m), ylabel Y (m) contains 34 objects of type line, text, patch. These objects represent Agent 1, Agent 2, Agent 3, Merged (PGO).

The merged map covers the combined exploration area of all three agents. Collaborative SLAM enables efficient mapping of large environments by distributing exploration across multiple robots and merging their observations at detected crossing points. The pose graph optimization step corrects accumulated drift by enforcing consistency at every crossing constraint.

Verify Map Alignment Across Agents

After processing all frames, inspect the final inter-agent transforms and crossing count. Each transform $T_{a_{k}}^{a_{1}}$ represents the rigid alignment from Agent 1's world frame to Agent $k$ 's world frame.

for agentIdx = 2:numAgents
    if isMerged(agentIdx) && ~isempty(agent2WorldTransforms{agentIdx})
        fprintf("Agent 1 to agent %d transform: [%.2f, %.2f, %.2f]\n", agentIdx, agent2WorldTransforms{agentIdx}.Translation);
    end
end

Agent 1 to agent 2 transform: [62.55, 0.26, 24.74]
Agent 1 to agent 3 transform: [-98.84, -0.10, -54.23]

fprintf("Total verified crossings: %d, All agents merged: %s\n", length(allCrossings), string(all(isMerged)));

Total verified crossings: 10, All agents merged: true

Supporting Functions

helperTryMergeAgents merges agent world frames using a verified crossing, propagating transitively through prior crossings to connect additional agents.

helperOptimizeMultiAgentTrajectories builds a factor graph with odometry and crossing constraints, then optimizes to produce globally consistent trajectories for all merged agents.

helperCheckCrossingDBoW2 queries another agent's DBoW2 database for visually similar keyframes, returning up to three ranked candidates.

helperVerifyCrossing prepares and geometrically verifies a crossing candidate by estimating the stereo relative pose and rejecting candidates with excessive rotation.

helperEstimateStereoRelativePose computes the metric-scale rigid transform between two stereo pairs using ORB feature matching, SGM disparity, 3-D reconstruction, and RANSAC registration.