-
Notifications
You must be signed in to change notification settings - Fork 83
/
main.m
96 lines (74 loc) · 3.56 KB
/
main.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
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
% This file contains a minimum working example demonstrating the use of the
% MATLAB distribution of SE-Sync, a certifiably correct algorithm for
% synchronization over the special Euclidean group
%
% Copyright (C) 2016 by David M. Rosen
%% Reset environment
clear all;
close all;
clc;
%% Import SE-Sync
run('../import_SE_Sync.m'); % It's that easy :-)!
%% Select dataset to run
data_dir = '../../data/'; % Relative path to directory containing example datasets
% 3D datasets
sphere2500 = 'sphere2500';
torus = 'torus3D';
grid = 'grid3D';
garage = 'parking-garage';
cubicle = 'cubicle';
rim = 'rim';
% 2D datasets
CSAIL = 'CSAIL';
manhattan = 'manhattan';
city10000 = 'city10000';
intel = 'intel';
ais = 'ais2klinik';
% Pick the dataset to run here
file = sphere2500;
g2o_file = strcat(data_dir, file, '.g2o');
%% Read in .g2o file
tic();
fprintf('Loading file: %s ...\n', g2o_file);
measurements = load_g2o_data(g2o_file);
t = toc();
num_poses = max(max(measurements.edges));
num_measurements = length(measurements.kappa);
d = length(measurements.t{1});
fprintf('Processed input file %s in %g seconds\n', g2o_file, t);
fprintf('Number of poses: %d\n', num_poses);
fprintf('Number of measurements: %d\n', num_measurements);
%% Set Manopt options (if desired)
Manopt_opts.tolgradnorm = 1e-2; % Stopping tolerance for norm of Riemannian gradient
Manopt_opts.rel_func_tol = 1e-5; % Additional stopping criterion for Manopt: stop if the relative function decrease between two successive accepted iterates is less than this value
Manopt_opts.miniter = 1; % Minimum number of outer iterations (i.e. accepted update steps) to perform
Manopt_opts.maxiter = 300; % Maximum number of outer iterations (i.e. accepted update steps) to perform
Manopt_opts.maxinner = 500; % Maximum number of iterations for the conjugate-gradient method used to compute approximate Newton steps
%manopt_options.maxtime = 60*60; % Maximum computation time to allow, in seconds
%manopt_options.solver = @steepestdescent; % Select Manopt solver to use: {trustregions (default), conjugategradient, steepestdescent}
%% Set SE-Sync options (if desired)
SE_Sync_opts.r0 = 5; % Initial maximum-rank parameter at which to start the Riemannian Staircase
SE_Sync_opts.rmax = 10; % Maximum maximum-rank parameter at which to terminate the Riemannian Staircase
SE_Sync_opts.eig_comp_rel_tol = 1e-4; % Relative tolerance for the minimum-eigenvalue computation used to test for second-order optimality with MATLAB's eigs() function
SE_Sync_opts.min_eig_lower_bound = -1e-3; % Minimum eigenvalue threshold for accepting a maxtrix as numerically positive-semidefinite
SE_Sync_opts.Cholesky = false; % Select whether to use Cholesky or QR decomposition to compute orthogonal projections
use_chordal_initialization = true; % Select whether to use the chordal initialization, or a random starting point
%% Run SE-Sync
% Pass explict settings for SE-Sync and/or Manopt, and use chordal
% initialization
fprintf('Computing chordal initialization...\n');
R = chordal_initialization(measurements);
Y0 = vertcat(R, zeros(SE_Sync_opts.r0 - d, num_poses*d));
[SDPval, Yopt, xhat, Fxhat, SE_Sync_info, problem_data] = SE_Sync(measurements, Manopt_opts, SE_Sync_opts, Y0);
% ... or ...
% Use default settings for everything
%[SDPval, Yopt, xhat, Fxhat, se_sync_info, problem_data] = SE_Sync(measurements);
%% Plot resulting solution
plot_loop_closures = true;
if plot_loop_closures
plot_poses(xhat.t, xhat.R, measurements.edges, '-b', .25);
else
plot_poses(xhat.t, xhat.R);
end
axis tight;
%view(90, -90); % For plotting 3D but nearly-planar datasets