-
Notifications
You must be signed in to change notification settings - Fork 0
/
helperGlobalBundleAdjustment.m
43 lines (33 loc) · 1.72 KB
/
helperGlobalBundleAdjustment.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
function [vSetKeyFrames, mapPointSet] = helperGlobalBundleAdjustment(...
vSetKeyFrames, mapPointSet, intrinsics, varargin)
% This is an example helper function that is subject to change or removal
% in future releases.
% Copyright 2019-2022 The MathWorks, Inc.
% Run full bundle adjustment on the first two key frames
tracks = findTracks(vSetKeyFrames);
cameraPoses = poses(vSetKeyFrames);
[pointIdx, validIdx] = findWorldPointsInTracks(mapPointSet, tracks);
xyzWorldPoints = mapPointSet.WorldPoints(pointIdx, :);
tracks = tracks(validIdx);
[refinedPoints, refinedAbsPoses] = bundleAdjustment(xyzWorldPoints, tracks, ...
cameraPoses, intrinsics, 'FixedViewIDs', 1, ...
'PointsUndistorted', true, 'AbsoluteTolerance', 1e-7,...
'RelativeTolerance', 1e-16, 'MaxIteration', 50, ...
'Solver', 'preconditioned-conjugate-gradient');
% Scale the map and the camera pose using the median depth of map points
medianDepth = median(vecnorm(refinedPoints.'));
refinedPoints = refinedPoints / medianDepth;
if nargin > 3
relPose = varargin{1};
refinedAbsPoses.AbsolutePose(end).Translation = ...
refinedAbsPoses.AbsolutePose(end).Translation / medianDepth;
relPose.Translation = relPose.Translation/medianDepth;
vSetKeyFrames = updateConnection(vSetKeyFrames, 1, 2, relPose);
end
% Update key frames with the refined poses
vSetKeyFrames = updateView(vSetKeyFrames, refinedAbsPoses);
% Update map points with the refined positions
mapPointSet = updateWorldPoints(mapPointSet, pointIdx, refinedPoints);
% Update view direction and depth
mapPointSet = updateRepresentativeView(mapPointSet, pointIdx, vSetKeyFrames.Views);
mapPointSet = updateLimitsAndDirection(mapPointSet, pointIdx, vSetKeyFrames.Views);