├── MATLAB ├── utils │ ├── io_utils │ │ ├── range_graph │ │ │ └── get_range_variable_mapping.m │ │ ├── pose_graph │ │ │ ├── rcvize_matrix.m │ │ │ ├── construct_pose_incidence_matrix.m │ │ │ └── construct_translational_matrices.m │ │ ├── save_experiment_results.m │ │ ├── get_experiment_filepaths_in_subdirs.m │ │ └── load_ra_slam_problem.m │ ├── preconditioners │ │ ├── get_ilu_preconditioner.m │ │ ├── get_ichol_preconditioner.m │ │ ├── get_jacobi_preconditioner.m │ │ ├── get_regularized_cholesky_preconditioner.m │ │ ├── precon_function_factory.m │ │ └── get_block_jacobi_preconditioner.m │ ├── manopt_utils │ │ ├── log_iterates.m │ │ └── Qproduct.m │ ├── math_utils │ │ ├── extract_translations_from_marginalized_solution.m │ │ ├── precfun.m │ │ ├── vector_to_symmetric_matrix.m │ │ └── check_value_is_valid.m │ ├── misc_utils │ │ ├── measurements_have_priors.m │ │ ├── get_robot_t_idxs.m │ │ └── get_robot_R_idxs.m │ └── viz_utils │ │ ├── viz_results.asv │ │ ├── align_solution_by_first_pose.m │ │ └── plot_solution.m ├── manopt │ ├── manopt │ │ ├── core │ │ │ ├── StoreDB.m │ │ │ ├── getStore.m │ │ │ ├── purgeStoredb.m │ │ │ ├── setStore.m │ │ │ ├── canGetEuclideanHessian.m │ │ │ ├── canGetSubgradient.m │ │ │ ├── canGetLinesearch.m │ │ │ ├── canGetCost.m │ │ │ ├── canGetDirectionalDerivative.m │ │ │ ├── canGetEuclideanGradient.m │ │ │ ├── canGetPrecon.m │ │ │ ├── canGetApproxGradient.m │ │ │ ├── canGetApproxHessian.m │ │ │ ├── canGetPartialEuclideanGradient.m │ │ │ ├── canGetPartialGradient.m │ │ │ ├── canGetHessian.m │ │ │ ├── mergeOptions.m │ │ │ ├── canGetSqrtPrecon.m │ │ │ ├── canGetGradient.m │ │ │ ├── handle_light.m │ │ │ └── getGlobalDefaults.m │ │ ├── tools │ │ │ ├── dexpm.m │ │ │ ├── dfunm.m │ │ │ ├── dlogm.m │ │ │ ├── dsqrtm.m │ │ │ ├── multiprod_legacy.m │ │ │ ├── multiskew.m │ │ │ ├── multiskewh.m │ │ │ ├── sinxoverx.m │ │ │ ├── multiherm.m │ │ │ ├── multisym.m │ │ │ ├── multitrace.m │ │ │ ├── orthogonalizetwice.m │ │ │ ├── multisqnorm.m │ │ │ ├── matrixlincomb.m │ │ │ ├── getsize.m │ │ │ ├── grammatrix.m │ │ │ ├── lyapunov_symmetric_eig.m │ │ │ ├── multiprodmultitransp_license.txt │ │ │ ├── identify_linear_piece.m │ │ │ ├── lincomb.m │ │ │ ├── tangent2vec.m │ │ │ ├── multiscale.m │ │ │ ├── multihconj_legacy.m │ │ │ ├── hashmd5.m │ │ │ └── sylvester_nochecks.m │ │ ├── autodiff │ │ │ ├── functions_AD │ │ │ │ ├── iscstruct.m │ │ │ │ ├── tocstruct.m │ │ │ │ ├── creal.m │ │ │ │ ├── cconj.m │ │ │ │ ├── cmat2col.m │ │ │ │ ├── ctransp.m │ │ │ │ ├── creshape.m │ │ │ │ ├── crepmat.m │ │ │ │ ├── cindex.m │ │ │ │ ├── cnormsqfro.m │ │ │ │ ├── cnormfro.m │ │ │ │ ├── csum.m │ │ │ │ ├── cdottimes.m │ │ │ │ ├── cplus.m │ │ │ │ ├── cminus.m │ │ │ │ ├── cprod.m │ │ │ │ ├── cdiag.m │ │ │ │ └── ctrace.m │ │ │ ├── findA_anchors.m │ │ │ └── basic_examples_AD │ │ │ │ ├── realtest_AD2.m │ │ │ │ ├── realtest_AD1.m │ │ │ │ └── realtest_AD3.m │ │ ├── manifolds │ │ │ ├── essential │ │ │ │ ├── privateessential │ │ │ │ │ ├── modAngle.m │ │ │ │ │ ├── essential_distMinAnglePair_discontinuityDistance.m │ │ │ │ │ ├── essential_closestRepresentative.m │ │ │ │ │ ├── essential_distMinAnglePair_computeDfBreak.m │ │ │ │ │ ├── essential_distMinAnglePair.m │ │ │ │ │ ├── essential_distMinAnglePair_ft.m │ │ │ │ │ ├── essential_distMinAnglePair_test.m │ │ │ │ │ ├── essential_distMinAnglePair_dfNewton.m │ │ │ │ │ └── essential_distMinAnglePair_ftFromQ.m │ │ │ │ ├── essential_flat.m │ │ │ │ ├── essential_sharp.m │ │ │ │ ├── essential_hat3.m │ │ │ │ ├── essential_costE2cost.m │ │ │ │ ├── essential_egradE2egrad.m │ │ │ │ └── essential_ehessE2ehess.m │ │ │ ├── multinomial │ │ │ │ ├── doubly_stochastic.m │ │ │ │ └── doubly_stochastic_general.m │ │ │ ├── ttfixedrank │ │ │ │ ├── weingarten_omega.mexw64 │ │ │ │ ├── TTeMPS_1.1 │ │ │ │ │ ├── @TTeMPS │ │ │ │ │ │ ├── subsref_mex.mexa64 │ │ │ │ │ │ ├── subsref_mex.mexw64 │ │ │ │ │ │ ├── subsref_mex.mexmaci64 │ │ │ │ │ │ ├── display.m │ │ │ │ │ │ ├── uplus.m │ │ │ │ │ │ ├── uminus.m │ │ │ │ │ │ ├── minus.m │ │ │ │ │ │ ├── mtimes.m │ │ │ │ │ │ ├── full.m │ │ │ │ │ │ ├── truncate.m │ │ │ │ │ │ ├── TTeMPS_to_TT.m │ │ │ │ │ │ ├── norm.m │ │ │ │ │ │ ├── round.m │ │ │ │ │ │ ├── gauge_matrices.m │ │ │ │ │ │ ├── orthogonalize.m │ │ │ │ │ │ ├── disp.m │ │ │ │ │ │ ├── left_orth_with_gauge.m │ │ │ │ │ │ ├── mergecore.m │ │ │ │ │ │ ├── plus.m │ │ │ │ │ │ ├── splitcore.m │ │ │ │ │ │ └── orth_at.m │ │ │ │ │ ├── @TTeMPS_tangent │ │ │ │ │ │ ├── TTeMPS_tangent_omega.mexa64 │ │ │ │ │ │ ├── TTeMPS_tangent_omega.mexw64 │ │ │ │ │ │ └── TTeMPS_tangent_omega.mexmaci64 │ │ │ │ │ ├── algorithms │ │ │ │ │ │ ├── completion │ │ │ │ │ │ │ ├── als_solve_mex.mexa64 │ │ │ │ │ │ │ ├── als_solve_mex.mexw64 │ │ │ │ │ │ │ ├── makeOmegaSet_mod.m │ │ │ │ │ │ │ ├── makeOmegaSet.m │ │ │ │ │ │ │ └── completion.m │ │ │ │ │ │ ├── increaseRank.m │ │ │ │ │ │ └── linearsystem │ │ │ │ │ │ │ ├── check_precond_laplace.m │ │ │ │ │ │ │ ├── solvePrecond.m │ │ │ │ │ │ │ ├── precond_rankOne.m │ │ │ │ │ │ │ └── alsLinsolve.m │ │ │ │ │ ├── @TTeMPS_tangent_orth │ │ │ │ │ │ ├── TTeMPS_tangent_orth_omega.mexa64 │ │ │ │ │ │ ├── TTeMPS_tangent_orth_omega.mexw64 │ │ │ │ │ │ ├── TTeMPS_tangent_orth_omega.mexmaci64 │ │ │ │ │ │ └── TTeMPS_tangent_orth_omega_openmp.mexa64 │ │ │ │ │ ├── @TTeMPS_op │ │ │ │ │ │ ├── display.m │ │ │ │ │ │ ├── mtimes.m │ │ │ │ │ │ ├── TTeMPS_op_to_TTeMPS.m │ │ │ │ │ │ ├── round.m │ │ │ │ │ │ ├── full.m │ │ │ │ │ │ ├── TTeMPS_op_to_TT_matrix.m │ │ │ │ │ │ ├── disp.m │ │ │ │ │ │ ├── plus.m │ │ │ │ │ │ └── contract.m │ │ │ │ │ ├── @TTeMPS_op_laplace │ │ │ │ │ │ ├── display.m │ │ │ │ │ │ ├── mtimes.m │ │ │ │ │ │ ├── TTeMPS_op_laplace_to_TT_matrix.m │ │ │ │ │ │ ├── TTeMPS_op_laplace_to_TTeMPS_op.m │ │ │ │ │ │ ├── disp.m │ │ │ │ │ │ ├── constr_precond_inner.m │ │ │ │ │ │ └── constr_precond.m │ │ │ │ │ ├── @TTeMPS_block │ │ │ │ │ │ ├── display.m │ │ │ │ │ │ ├── uplus.m │ │ │ │ │ │ ├── uminus.m │ │ │ │ │ │ ├── minus.m │ │ │ │ │ │ ├── round.m │ │ │ │ │ │ ├── truncate.m │ │ │ │ │ │ ├── norm.m │ │ │ │ │ │ ├── full.m │ │ │ │ │ │ ├── mtimes.m │ │ │ │ │ │ ├── disp.m │ │ │ │ │ │ ├── orthogonalize.m │ │ │ │ │ │ └── innerprod.m │ │ │ │ │ ├── README_MANOPT.txt │ │ │ │ │ ├── operators │ │ │ │ │ │ └── laplace.m │ │ │ │ │ ├── TTeMPS_randn.m │ │ │ │ │ ├── trunc_singular.m │ │ │ │ │ ├── TTeMPS_rand.m │ │ │ │ │ ├── TT_to_TTeMPS.m │ │ │ │ │ ├── unfold.m │ │ │ │ │ ├── matricize.m │ │ │ │ │ ├── examples │ │ │ │ │ │ └── example.m │ │ │ │ │ ├── tensorize.m │ │ │ │ │ ├── tensorprod_ttemps.m │ │ │ │ │ └── install_mex.m │ │ │ │ └── install_mex.m │ │ │ ├── rotations │ │ │ │ ├── randherm.m │ │ │ │ ├── randskewh.m │ │ │ │ ├── randunitary.m │ │ │ │ ├── randskew.m │ │ │ │ └── randrot.m │ │ │ └── fixedranktensors │ │ │ │ └── tucker2multiarray.m │ │ └── solvers │ │ │ ├── neldermead │ │ │ └── centroid.m │ │ │ └── trustregions │ │ │ └── license for original GenRTR code.txt │ ├── examples │ │ ├── positive_definite_karcher_mean.m │ │ └── run_all_examples.m │ ├── manopt_version.m │ ├── checkinstall │ │ └── basicexample.m │ ├── CREDITS.txt │ ├── importmanopt.m │ └── LICENSE.txt ├── paper_plots │ └── utils │ │ ├── get_suboptimality_gaps_from_results_fpaths.m │ │ ├── get_num_poses_from_fpath_names.m │ │ ├── get_num_ranges_from_fpath_names.m │ │ ├── get_num_robots_from_fpath_names.m │ │ └── get_range_covs_from_fpath_names.m └── lobpcg │ └── README.md ├── media ├── pz1_grey_solid_inf_loop.gif └── pz2_grey_solid_inf_loop.gif ├── CITATION.cff ├── README.md └── .gitignore /MATLAB/utils/io_utils/range_graph/get_range_variable_mapping.m: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /media/pz1_grey_solid_inf_loop.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/media/pz1_grey_solid_inf_loop.gif -------------------------------------------------------------------------------- /media/pz2_grey_solid_inf_loop.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/media/pz2_grey_solid_inf_loop.gif -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/StoreDB.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/core/StoreDB.m -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/dexpm.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/tools/dexpm.m -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/dfunm.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/tools/dfunm.m -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/dlogm.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/tools/dlogm.m -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/dsqrtm.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/tools/dsqrtm.m -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multiprod_legacy.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/tools/multiprod_legacy.m -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/iscstruct.m: -------------------------------------------------------------------------------- 1 | function flag = iscstruct(A) 2 | flag = ( isstruct(A) && isfield(A, 'real') && isfield(A, 'imag') ); 3 | end 4 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/privateessential/modAngle.m: -------------------------------------------------------------------------------- 1 | %Maps any angle to the equivalent between -pi and pi 2 | function a=modAngle(a) 3 | a=mod(a+pi,2*pi)-pi; 4 | -------------------------------------------------------------------------------- /MATLAB/utils/preconditioners/get_ilu_preconditioner.m: -------------------------------------------------------------------------------- 1 | function precon_function = get_ilu_preconditioner(A) 2 | [L, U] = ilu(A); 3 | precon_function = @(x) L \ (U \ x); 4 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/essential_flat.m: -------------------------------------------------------------------------------- 1 | function Hp = essential_flat(H) 2 | %Reshape a [3x6xk] matrix to a [3x3x2k] matrix 3 | Hp = reshape(H,3,3,[]); 4 | end 5 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/multinomial/doubly_stochastic.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/multinomial/doubly_stochastic.m -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/essential_sharp.m: -------------------------------------------------------------------------------- 1 | function H = essential_sharp(Hp) 2 | %Reshape a [3x3x2k] matrix to a [3x6xk] matrix 3 | H = reshape(Hp,3,6,[]); 4 | end 5 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/weingarten_omega.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/weingarten_omega.mexw64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/multinomial/doubly_stochastic_general.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/multinomial/doubly_stochastic_general.m -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/getStore.m: -------------------------------------------------------------------------------- 1 | function store = getStore(problem, x, storedb) %#ok 2 | 3 | error('This file was removed from Manopt. Please use the StoreDB class.'); 4 | 5 | end 6 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/purgeStoredb.m: -------------------------------------------------------------------------------- 1 | function storedb = purgeStoredb(storedb, storedepth) %#ok 2 | 3 | error('This file was removed from Manopt. Please use the StoreDB class.'); 4 | 5 | end 6 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/setStore.m: -------------------------------------------------------------------------------- 1 | function storedb = setStore(problem, x, storedb, store) %#ok 2 | 3 | error('This file was removed from Manopt. Please use the StoreDB class.'); 4 | 5 | end 6 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/subsref_mex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/subsref_mex.mexa64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/subsref_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/subsref_mex.mexw64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/subsref_mex.mexmaci64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/subsref_mex.mexmaci64 -------------------------------------------------------------------------------- /MATLAB/utils/preconditioners/get_ichol_preconditioner.m: -------------------------------------------------------------------------------- 1 | function precon_function = get_ichol_preconditioner(Q) 2 | ichol_opts.diagcomp = 1e-1; 3 | L = ichol(Q, ichol_opts); 4 | LT = L'; 5 | precon_function = @(x) LT \ (L \ x); 6 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent/TTeMPS_tangent_omega.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent/TTeMPS_tangent_omega.mexa64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent/TTeMPS_tangent_omega.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent/TTeMPS_tangent_omega.mexw64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/completion/als_solve_mex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/completion/als_solve_mex.mexa64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/completion/als_solve_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/completion/als_solve_mex.mexw64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent/TTeMPS_tangent_omega.mexmaci64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent/TTeMPS_tangent_omega.mexmaci64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent_orth/TTeMPS_tangent_orth_omega.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent_orth/TTeMPS_tangent_orth_omega.mexa64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent_orth/TTeMPS_tangent_orth_omega.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent_orth/TTeMPS_tangent_orth_omega.mexw64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent_orth/TTeMPS_tangent_orth_omega.mexmaci64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent_orth/TTeMPS_tangent_orth_omega.mexmaci64 -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent_orth/TTeMPS_tangent_orth_omega_openmp.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MarineRoboticsGroup/cora-matlab/HEAD/MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_tangent_orth/TTeMPS_tangent_orth_omega_openmp.mexa64 -------------------------------------------------------------------------------- /MATLAB/utils/manopt_utils/log_iterates.m: -------------------------------------------------------------------------------- 1 | function stats = log_iterates(problem, x, stats) 2 | % function stats = log_iterates(problem, x, stats) 3 | % 4 | % This small helper function is used to log the sequence of iterates 5 | % generated by the Riemannian Staircase 6 | 7 | stats.Xvals = x'; 8 | 9 | end 10 | 11 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/tocstruct.m: -------------------------------------------------------------------------------- 1 | function B = tocstruct(A) 2 | if iscstruct(A) 3 | B = A; 4 | elseif isnumeric(A) 5 | B.real = real(A); 6 | B.imag = imag(A); 7 | else 8 | error('Input does not have the expected format.'); 9 | end 10 | end 11 | -------------------------------------------------------------------------------- /MATLAB/manopt/examples/positive_definite_karcher_mean.m: -------------------------------------------------------------------------------- 1 | function X = positive_definite_karcher_mean(A) 2 | 3 | warning(['positive_definite_karcher_mean is now named ' ... 4 | 'positive_definite_intrinsic_mean.']); 5 | 6 | if exist('A', 'var') 7 | X = positive_definite_intrinsic_mean(A); 8 | end 9 | 10 | end 11 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: If you use this software, please cite it as below. 3 | authors: 4 | - family-names: Papalia 5 | given-names: Alan 6 | orcid: https://orcid.org/0000-0002-6922-135X 7 | title: Certifiably Correct Range-Aided SLAM 8 | version: 0.0.1 9 | date-released: 2023-04-28 10 | url: "https://github.com/MarineRoboticsGroup/cora" 11 | -------------------------------------------------------------------------------- /MATLAB/utils/math_utils/extract_translations_from_marginalized_solution.m: -------------------------------------------------------------------------------- 1 | function Xfull = extract_translations_from_marginalized_solution(X, problem) 2 | % t* = - (X*)' * Qxy * Qyy^{-1}; 3 | assert(problem.use_marginalized, 'Q must be marginalized to extract translations'); 4 | 5 | translations = - (X' * problem.LeftOperator) / problem.Ltrans; 6 | Xfull = [X; translations']; 7 | end -------------------------------------------------------------------------------- /MATLAB/utils/preconditioners/get_jacobi_preconditioner.m: -------------------------------------------------------------------------------- 1 | function precon_function = get_jacobi_preconditioner(A) 2 | diagonal = diag(A); 3 | 4 | % Create a diagonal preconditioning matrix D with diagonal elements as reciprocals 5 | D = spdiags(1 ./ diagonal, 0, size(A, 1), size(A, 2)); 6 | 7 | % Create a function handle to the preconditioner 8 | precon_function = @(x) D * x; 9 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op/display.m: -------------------------------------------------------------------------------- 1 | function display(x) 2 | %DISPLAY Display TT/MPS tensor as a tensor network 3 | % 4 | % TTeMPS Toolbox. 5 | % Michael Steinlechner, 2013-2016 6 | % Questions and contact: michael.steinlechner@epfl.ch 7 | % BSD 2-clause license, see LICENSE.txt 8 | 9 | disp( x, inputname(1)); 10 | end 11 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op_laplace/display.m: -------------------------------------------------------------------------------- 1 | function display(x) 2 | %DISP Display TT/MPS tensor as a tensor network 3 | % 4 | % TTeMPS Toolbox. 5 | % Michael Steinlechner, 2013-2016 6 | % Questions and contact: michael.steinlechner@epfl.ch 7 | % BSD 2-clause license, see LICENSE.txt 8 | 9 | disp( x, inputname(1)); 10 | end 11 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/display.m: -------------------------------------------------------------------------------- 1 | function display(x) 2 | %DISP Display TT/MPS tensor as a tensor network 3 | % 4 | 5 | % TTeMPS Toolbox. 6 | % Michael Steinlechner, 2013-2016 7 | % Questions and contact: michael.steinlechner@epfl.ch 8 | % BSD 2-clause license, see LICENSE.txt 9 | 10 | disp( x, inputname(1)); 11 | end 12 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/display.m: -------------------------------------------------------------------------------- 1 | function display(x) 2 | %DISP Display TT/MPS tensor as a tensor network 3 | % 4 | 5 | % TTeMPS Toolbox. 6 | % Michael Steinlechner, 2013-2016 7 | % Questions and contact: michael.steinlechner@epfl.ch 8 | % BSD 2-clause license, see LICENSE.txt 9 | 10 | disp( x, inputname(1)); 11 | end 12 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/uplus.m: -------------------------------------------------------------------------------- 1 | function x = uplus( x ) 2 | %UPLUS Unary plus. 3 | % X = UPLUS(X) does nothing. 4 | % 5 | % See also UMINUS. 6 | 7 | % TTeMPS Toolbox. 8 | % Michael Steinlechner, 2013-2016 9 | % Questions and contact: michael.steinlechner@epfl.ch 10 | % BSD 2-clause license, see LICENSE.txt 11 | 12 | end 13 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/uplus.m: -------------------------------------------------------------------------------- 1 | function x = uplus( x ) 2 | %UPLUS Unary plus. 3 | % X = UPLUS(X) does nothing. 4 | % 5 | % See also UMINUS. 6 | 7 | % TTeMPS Toolbox. 8 | % Michael Steinlechner, 2013-2016 9 | % Questions and contact: michael.steinlechner@epfl.ch 10 | % BSD 2-clause license, see LICENSE.txt 11 | 12 | end 13 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/privateessential/essential_distMinAnglePair_discontinuityDistance.m: -------------------------------------------------------------------------------- 1 | function [tBreak,a,b,c,m,p]=essential_distMinAnglePair_discontinuityDistance(Q21) 2 | a=Q21(1,1)+Q21(2,2); 3 | b=Q21(1,2)-Q21(2,1); 4 | c=Q21(3,3); 5 | 6 | m=norm([a;b]); 7 | p=sign(a)*acos(clip(b/m)); 8 | 9 | %tBreak=modAngle(3/2*pi-p); 10 | tBreak=-0.5*pi-p; 11 | 12 | function v=clip(v) 13 | v=min(1,max(-1,v)); 14 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt_version.m: -------------------------------------------------------------------------------- 1 | function [version, released] = manopt_version() 2 | % Returns the version of the Manopt package you are running, as a vector. 3 | % 4 | % function [version, released] = manopt_version() 5 | % 6 | % version(1) is the primary version number. 7 | % released is the date this version was released, in the same format as the 8 | % date() function in Matlab. 9 | 10 | version = [7, 1, 0]; 11 | released = '30-Sep-2022'; 12 | 13 | end 14 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/install_mex.m: -------------------------------------------------------------------------------- 1 | % Compiles weingarten_omega.c into a binary mex file. 2 | % Requires Matlab to have access to a compatible C compiler. 3 | % If need be, run 'mex -setup' for help installing a compiler. 4 | 5 | % This file is part of Manopt: www.manopt.org. 6 | % Original author: Michael Psenka, Nov. 24, 2020. 7 | % Contributors: Nicolas Boumal 8 | % Change log: 9 | 10 | mex -lmwlapack -lmwblas -largeArrayDims weingarten_omega.c 11 | -------------------------------------------------------------------------------- /MATLAB/utils/misc_utils/measurements_have_priors.m: -------------------------------------------------------------------------------- 1 | function has_priors = measurements_have_priors(measurements) 2 | % check if measurements.pose_priors or measurements.landmark_priors exist and are not empty 3 | has_pose_priors = isfield(measurements, 'pose_priors') && ~isempty(measurements.pose_priors.mean); 4 | has_landmark_priors = isfield(measurements, 'landmark_priors') && ~isempty(measurements.landmark_priors.mean); 5 | has_priors = has_pose_priors || has_landmark_priors; 6 | end -------------------------------------------------------------------------------- /MATLAB/utils/manopt_utils/Qproduct.m: -------------------------------------------------------------------------------- 1 | function QX = Qproduct(X, problem_data) 2 | if ~problem_data.use_marginalized 3 | QX = problem_data.Q * X; 4 | else 5 | QmainX = problem_data.Qmain * X; 6 | 7 | QtransX = problem_data.LeftOperatorRed * ... 8 | (problem_data.LtransCholRed' \ ... 9 | (problem_data.LtransCholRed \ ... 10 | (problem_data.LeftOperatorRed' * X))); 11 | 12 | QX = QmainX - QtransX; 13 | end 14 | 15 | end 16 | 17 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/uminus.m: -------------------------------------------------------------------------------- 1 | function x = uminus( x ) 2 | %UMINUS Unary minus. 3 | % X = UMINUS(X) returns the negated TTeMPS tensor X. Only the first core 4 | % is touched. 5 | % 6 | % See also UPLUS. 7 | 8 | % TTeMPS Toolbox. 9 | % Michael Steinlechner, 2013-2016 10 | % Questions and contact: michael.steinlechner@epfl.ch 11 | % BSD 2-clause license, see LICENSE.txt 12 | 13 | 14 | x.U{1} = -x.U{1}; 15 | end 16 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/uminus.m: -------------------------------------------------------------------------------- 1 | function x = uminus( x ) 2 | %UMINUS Unary minus. 3 | % X = UMINUS(X) returns the negated TTeMPS block-mu tensor X. Only the supercore 4 | % is touched. 5 | % 6 | % See also UPLUS. 7 | 8 | % TTeMPS Toolbox. 9 | % Michael Steinlechner, 2013-2016 10 | % Questions and contact: michael.steinlechner@epfl.ch 11 | % BSD 2-clause license, see LICENSE.txt 12 | 13 | 14 | x.U{x.mu} = -x.U{x.mu}; 15 | end 16 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/privateessential/essential_closestRepresentative.m: -------------------------------------------------------------------------------- 1 | function Q2r=essential_closestRepresentative(Q1,Q2,varargin) 2 | [tMin,~,Q2]=essential_distMinAngle(Q1,Q2,varargin{:}); 3 | NQ1=size(Q1,3); 4 | NQ2=size(Q2,3); 5 | 6 | if NQ1>1 && NQ2==1 7 | Q2=repmat(Q2,[1 1 NQ1]); 8 | end 9 | NQ=max(NQ1,NQ2); 10 | 11 | Q2r=zeros(size(Q2)); 12 | for iQ=1:NQ 13 | t=tMin(iQ); 14 | Rz=[cos(t) -sin(t) 0; sin(t) cos(t) 0; 0 0 1]; 15 | Q2r(1:3,1:3,iQ)=Rz*Q2(1:3,1:3,iQ); 16 | Q2r(4:6,1:3,iQ)=Rz*Q2(4:6,1:3,iQ); 17 | end 18 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/minus.m: -------------------------------------------------------------------------------- 1 | function z = minus( x, y ) 2 | %MINUS Substraction of two TT/MPS tensors. 3 | % Z = MINUS(X,Y) substracts two TT/MPS tensors. The rank of the resulting 4 | % tensor is the sum of the individual ranks. 5 | % 6 | % See also PLUS, UMINUS. 7 | 8 | % TTeMPS Toolbox. 9 | % Michael Steinlechner, 2013-2016 10 | % Questions and contact: michael.steinlechner@epfl.ch 11 | % BSD 2-clause license, see LICENSE.txt 12 | 13 | z = plus(x, uminus(y)); 14 | 15 | end 16 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/README_MANOPT.txt: -------------------------------------------------------------------------------- 1 | This is a modified version of the original package TTeMPS_1.1, 2 | original authored by Michael Steinlechner. Please see LICENSE.txt in the 3 | same directory for information of the original author and license. 4 | 5 | This version contains modifications in order to help some modules work 6 | better with manopt. 7 | 8 | Link to original package: https://www.epfl.ch/labs/anchp/index-html/software/ttemps/ 9 | 10 | Modifications author: Michael Psenka, Nov. 24, 2020. 11 | Modifications contributors: Nicolas Boumal. -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/minus.m: -------------------------------------------------------------------------------- 1 | function z = minus( x, y ) 2 | %MINUS Substraction of two TT/MPS block-mu tensors. 3 | % Z = MINUS(X,Y) substracts two TT/MPS tensors. The rank of the resulting 4 | % tensor is the sum of the individual ranks. 5 | % 6 | % See also PLUS, UMINUS. 7 | 8 | % TTeMPS Toolbox. 9 | % Michael Steinlechner, 2013-2016 10 | % Questions and contact: michael.steinlechner@epfl.ch 11 | % BSD 2-clause license, see LICENSE.txt 12 | 13 | z = plus(x, uminus(y)); 14 | 15 | end 16 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multiskew.m: -------------------------------------------------------------------------------- 1 | function Y = multiskew(X) 2 | % Returns the skew-symmetric parts of the matrices in the 3D matrix X. 3 | % 4 | % function Y = multiskew(X) 5 | % 6 | % Y is a 3D matrix the same size as X. Each slice Y(:, :, i) is the 7 | % skew-symmetric part of the slice X(:, :, i). 8 | % 9 | % See also: multiskewh multiprod multitransp multiscale multisym 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, Jan. 31, 2013. 13 | % Contributors: 14 | % Change log: 15 | 16 | Y = .5*(X - multitransp(X)); 17 | 18 | end 19 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multiskewh.m: -------------------------------------------------------------------------------- 1 | function Y = multiskewh(X) 2 | % Returns the skew-Hermitian parts of the matrices in the 3D matrix X. 3 | % 4 | % function Y = multiskewh(X) 5 | % 6 | % Y is a 3D matrix the same size as X. Each slice Y(:, :, i) is the 7 | % skew-Hermitian part of the slice X(:, :, i). 8 | % 9 | % See also: multiskew multisym multiherm multiprod multitransp multiscale 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, June 18, 2019. 13 | % Contributors: 14 | % Change log: 15 | 16 | Y = .5*(X - multihconj(X)); 17 | 18 | end 19 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/sinxoverx.m: -------------------------------------------------------------------------------- 1 | function y = sinxoverx(x) 2 | % Computes sin(x) ./ x entrywise, such that sin(0) / 0 yields 1. 3 | % 4 | % function y = sinxoverx(x) 5 | % 6 | % The function sinc from the Signal Processing Toolbox is related to this 7 | % function via sinc(x) = sinxoverx(pi*x). 8 | % 9 | % See also: sinc 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, Jan. 12, 2022. 13 | % Contributors: 14 | % Change log: 15 | 16 | y = sin(x) ./ x; % By default, divisions by zero trigger no warnings. 17 | y(x == 0) = 1; 18 | 19 | end 20 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/operators/laplace.m: -------------------------------------------------------------------------------- 1 | %LAPLACE Construct a simple d-dimensional Laplace operator and return TTeMPS_op_laplace object. 2 | 3 | % TTeMPS Toolbox. 4 | % Michael Steinlechner, 2013-2016 5 | % Questions and contact: michael.steinlechner@epfl.ch 6 | % BSD 2-clause license, see LICENSE.txt 7 | 8 | function L = laplace( I, n, d ); 9 | 10 | one = ones(n,1); 11 | q = linspace( I(1), I(2), n)'; 12 | h = abs(q(2) - q(1)); 13 | L0 = -spdiags( [one, -2*one, one], [-1 0 1], n, n) / (h^2); 14 | 15 | L = TTeMPS_op_laplace( L0, d ); 16 | end 17 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/mtimes.m: -------------------------------------------------------------------------------- 1 | function x = mtimes( a, x ) 2 | %MINUS Multiplication of TT/MPS tensor by scalar. 3 | % X = MTIMES(A, X) multiplies the TT/MPS tensor X by the scalar A. 4 | % 5 | % See also PLUS, MINUS. 6 | 7 | % TTeMPS Toolbox. 8 | % Michael Steinlechner, 2013-2016 9 | % Questions and contact: michael.steinlechner@epfl.ch 10 | % BSD 2-clause license, see LICENSE.txt 11 | 12 | %x.U{1} = a*x.U{1}; 13 | 14 | c = a^(1/x.order); 15 | for i = 1:x.order 16 | x.U{i} = c*x.U{i}; 17 | end 18 | end 19 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetEuclideanHessian.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetEuclideanHessian(problem) 2 | % Checks whether the Euclidean Hessian can be computed for a problem. 3 | % 4 | % function candoit = canGetEuclideanHessian(problem) 5 | % 6 | % Returns true if the Euclidean Hessian can be computed given the problem 7 | % description, false otherwise. 8 | % 9 | % See also: canGetHessian getEuclideanGradient 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, July 24, 2018. 13 | % Contributors: 14 | % Change log: 15 | 16 | 17 | candoit = isfield(problem, 'ehess'); 18 | 19 | end 20 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetSubgradient.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetSubgradient(problem) 2 | % Checks whether a subgradient can be computed for a problem structure. 3 | % 4 | % function candoit = canGetSubgradient(problem) 5 | % 6 | % Returns true if a subgradient of the cost function can be computed given 7 | % the problem description, false otherwise. 8 | % 9 | % See also: canGetGradient 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, July 20, 2017. 13 | % Contributors: 14 | % Change log: 15 | 16 | candoit = isfield(problem, 'subgrad') || canGetGradient(problem); 17 | 18 | end 19 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetLinesearch.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetLinesearch(problem) 2 | % Checks whether the problem structure can give a line-search a hint. 3 | % 4 | % function candoit = canGetLinesearch(problem) 5 | % 6 | % Returns true if the the problem description includes a mechanism to give 7 | % line-search algorithms a hint as to "how far to look", false otherwise. 8 | % 9 | % See also: getLinesearch 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, July 17, 2014. 13 | % Contributors: 14 | % Change log: 15 | 16 | 17 | candoit = isfield(problem, 'linesearch'); 18 | 19 | end 20 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/privateessential/essential_distMinAnglePair_computeDfBreak.m: -------------------------------------------------------------------------------- 1 | function dfBreak=essential_distMinAnglePair_computeDfBreak(tBreak,Q21) 2 | c=cos(tBreak); 3 | s=sin(tBreak); 4 | 5 | % The code below is an optimization exploiting the structure of RBreak to 6 | % substitute the following code 7 | % RBreak=Q1'*[c -s 0; s c 0; 0 0 1]*Q2; 8 | % 9 | % %compute v0 such that RBreak=rot(pi*v0) 10 | % [U,~,~]=svd(RBreak+eye(3)); 11 | % v0=U(:,1); 12 | % 13 | % dfBreak=pi*abs(Q1(3,:)*v0); 14 | 15 | Q1RBreakQ1=[c -s 0; s c 0; 0 0 1]*Q21; 16 | [U,~,~]=svd(Q1RBreakQ1+eye(3)); 17 | dfBreak=pi*abs(U(3,1)); 18 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/round.m: -------------------------------------------------------------------------------- 1 | function x = round( x, tol ) 2 | %ROUND Approximate TTeMPS tensor within a prescribed tolerance. 3 | % X = ROUND( X, tol ) truncates the given TTeMPS tensor X to a 4 | % lower rank such that the error is in order of tol. 5 | 6 | % TTeMPS Toolbox. 7 | % Michael Steinlechner, 2013-2016 8 | % Questions and contact: michael.steinlechner@epfl.ch 9 | % BSD 2-clause license, see LICENSE.txt 10 | 11 | y = TTeMPS_block_to_TTeMPS( x ); 12 | y = round(y , tol); 13 | x = TTeMPS_block.TTeMPS_to_TTeMPS_block( y, x.mu, x.p ); 14 | 15 | end 16 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/truncate.m: -------------------------------------------------------------------------------- 1 | function x = round( x, r ) 2 | %ROUND Approximate TTeMPS tensor within a prescribed tolerance. 3 | % X = ROUND( X, tol ) truncates the given TTeMPS tensor X to a 4 | % lower rank such that the error is in order of tol. 5 | 6 | % TTeMPS Toolbox. 7 | % Michael Steinlechner, 2013-2016 8 | % Questions and contact: michael.steinlechner@epfl.ch 9 | % BSD 2-clause license, see LICENSE.txt 10 | 11 | y = TTeMPS_block_to_TTeMPS( x ); 12 | y = truncate(y , r); 13 | x = TTeMPS_block.TTeMPS_to_TTeMPS_block( y, x.mu, x.p ); 14 | 15 | end 16 | -------------------------------------------------------------------------------- /MATLAB/utils/io_utils/pose_graph/rcvize_matrix.m: -------------------------------------------------------------------------------- 1 | function [rows, cols, vals] = rcvize_matrix(M, i, j) 2 | %function [rows, cols, vals] = rcvize_matrix(M, i, j) 3 | % 4 | % Given a DxD matrix M that we wish to insert into the (i,j)th DxD 5 | % block of a sparse matrix, this function computes and returns the 6 | % corresponding {row, col, val} vectors describing this block 7 | 8 | % Copyright (C) 2016 by David M. Rosen 9 | 10 | D = size(M, 1); 11 | 12 | vals = reshape(M, [1, D^2]); %Vectorize M by concatenating its columns 13 | 14 | rows = repmat( [D*(i-1) + 1 : D*(i-1) + D], [1,D]); 15 | cols = kron( [D*(j-1) + 1 : D*(j-1) + D], ones(1, D)); 16 | 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /MATLAB/utils/misc_utils/get_robot_t_idxs.m: -------------------------------------------------------------------------------- 1 | function t_idxs = get_robot_t_idxs(problem_data, robot_idx) 2 | 3 | % has_priors = (problem_data.num_pose_priors + problem_data.num_landmark_priors) > 0; 4 | warning("Assuming no priors - get_robot t_idxs") 5 | has_priors = 0; 6 | 7 | num_robot_poses = problem_data.num_poses-has_priors; %remove the aux pose 8 | num_robots = problem_data.num_robots; 9 | poses_per_robot = num_robot_poses / num_robots; 10 | 11 | % skip the aux pose 12 | all_robot_t_idxs = problem_data.all_t_idxs(has_priors+1:end); 13 | t_idxs = all_robot_t_idxs(poses_per_robot*(robot_idx-1)+1:poses_per_robot*robot_idx); 14 | 15 | end -------------------------------------------------------------------------------- /MATLAB/utils/math_utils/precfun.m: -------------------------------------------------------------------------------- 1 | function y = precfun(x) 2 | 3 | global P 4 | global PT 5 | global D 6 | global L 7 | global LT 8 | 9 | % A = scale * S; 10 | % A = A(p, :); 11 | % A = L \ A; 12 | % A = D_orig \ A; 13 | % A = L' \ A; 14 | % A = scale * A(:, p); 15 | % A = P * A; 16 | 17 | has_scale = 0; 18 | % T = transpose(P) * transpose(L_inv) * D * L_inv * P; 19 | % y = T * x; (but never form T explicitly) 20 | if has_scale 21 | % x1 = scale*x; 22 | % x2 = (LT \ (D* (L \ x1(p, :)))); 23 | % y = scale * (x2(p, :)); 24 | else 25 | % x1 = (LT \ (D * (L \ x(p, :)))); 26 | % y = x1(p, :); 27 | y = P * (LT \ (D * (L \ (PT * x)))); 28 | end 29 | end 30 | 31 | -------------------------------------------------------------------------------- /MATLAB/utils/viz_utils/viz_results.asv: -------------------------------------------------------------------------------- 1 | clc; clear all; 2 | base_data_dir = "~/data/manhattan/cert/no_loop_closures"; 3 | base_data_dir = "~/data/hat_data/16OCT2022"; 4 | 5 | experiments = get_experiment_filepaths_in_subdirs(base_data_dir); 6 | num_experiments = length(experiments); 7 | 8 | data_path = experiments(1); 9 | res_path = strrep(data_path, ".mat", "_results.mat"); 10 | solver_info_path = strrep(data_path, ".mat", "_solver_info.mat"); 11 | 12 | load(res_path); 13 | exp_data = load_ra_slam_problem(data_path); 14 | 15 | %% 16 | clc; clf; 17 | figure(2); 18 | hold on; 19 | Xvals_rounded = round_solution(res.Xvals, problem); 20 | %% 21 | for idx = 1:length(res) 22 | 23 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetCost.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetCost(problem) 2 | % Checks whether the cost function can be computed for a problem structure. 3 | % 4 | % function candoit = canGetCost(problem) 5 | % 6 | % Returns true if the cost function can be computed given the problem 7 | % description, false otherwise. 8 | % 9 | % See also: getCost canGetDirectionalDerivative canGetGradient canGetHessian 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, Dec. 30, 2012. 13 | % Contributors: 14 | % Change log: 15 | 16 | 17 | candoit = isfield(problem, 'cost') || isfield(problem, 'costgrad'); 18 | 19 | end 20 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multiherm.m: -------------------------------------------------------------------------------- 1 | function Y = multiherm(X) 2 | % Returns the Hermitian parts of the matrices in a 3D array 3 | % 4 | % function Y = multiherm(X) 5 | % 6 | % Y is a 3D array the same size as X. Each slice Y(:, :, i) is the 7 | % Hermitian part of the slice X(:, :, i), that is, 8 | % 9 | % Y(:, :, i) = .5*(X(:, :, i) + X(:, :, i)') 10 | % 11 | % See also: multisym multiskew multiskewh multihconj multiprod multitransp multiscale 12 | 13 | % This file is part of Manopt: www.manopt.org. 14 | % Original author: Hiroyuki Sato, April 27, 2015. 15 | % Contributors: 16 | % Change log: 17 | 18 | Y = .5*(X + multihconj(X)); 19 | 20 | end 21 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op/mtimes.m: -------------------------------------------------------------------------------- 1 | function A = mtimes( B, A ) 2 | %MINUS Multiplication of TT/MPS operator by scalar. 3 | % A = MTIMES(B, A) multiplies the TT/MPS operator A by the scalar B. 4 | % 5 | % See also PLUS. 6 | 7 | % TTeMPS Toolbox. 8 | % Michael Steinlechner, 2013-2016 9 | % Questions and contact: michael.steinlechner@epfl.ch 10 | % BSD 2-clause license, see LICENSE.txt 11 | 12 | %A.U{1} = B*A.U{1}; 13 | %Numerically more stable way: distribute scalar over all cores: 14 | 15 | b = B^(1/A.order); 16 | for i = 1:A.order 17 | A.U{i} = b*A.U{i}; 18 | end 19 | end 20 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/essential_hat3.m: -------------------------------------------------------------------------------- 1 | %Compute the matrix representation of the cross product 2 | %function [V,vShift] = essential_hat3(v) 3 | %V is a [3x3xN] array of skew-symmetric matrices where each [3x3] block is 4 | %the matrix representation of the cross product of one of the columns of v 5 | %vShift is equal to permute(v,[1 3 2]). 6 | function [V, vShift] = essential_hat3(v) 7 | N = size(v,2); 8 | V = zeros(3,3,N); 9 | vShift = permute(v,[1 3 2]); 10 | V(1,2,:) = -vShift(3,:,:); 11 | V(2,1,:) = vShift(3,:,:); 12 | V(1,3,:) = vShift(2,:,:); 13 | V(3,1,:) = -vShift(2,:,:); 14 | V(2,3,:) = -vShift(1,:,:); 15 | V(3,2,:) = vShift(1,:,:); 16 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/increaseRank.m: -------------------------------------------------------------------------------- 1 | % Helper routine to artificially increase the rank of the current iterate. 2 | 3 | % TTeMPS Toolbox. 4 | % Michael Steinlechner, 2013-2016 5 | % Questions and contact: michael.steinlechner@epfl.ch 6 | % BSD 2-clause license, see LICENSE.txt 7 | 8 | function X = increaseRank( X, inc, idx, epsilon ) 9 | 10 | r = X.rank; 11 | n = X.size; 12 | d = X.order; 13 | 14 | if ~exist('epsilon','var') 15 | epsilon = 1e-8; 16 | end 17 | 18 | X.U{idx-1} = cat( 3, X.U{idx-1}, epsilon*randn(r(idx-1), n(idx-1), inc) ); 19 | X.U{idx} = cat( 1, X.U{idx}, epsilon*randn(inc, n(idx), r(idx+1)) ); 20 | end 21 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/privateessential/essential_distMinAnglePair.m: -------------------------------------------------------------------------------- 1 | function [tMin,fMin,tBreak1,tBreak2,Q2,tMinAll]=essential_distMinAnglePair(Q1,Q2,kFlip) 2 | 3 | switch kFlip 4 | case 1 5 | %nothing to do 6 | case 2 7 | Q2([2 3 4 6],:)=-Q2([2 3 4 6],:); 8 | case 3 9 | Q2([4 5],:)=-Q2([4 5],:); 10 | case 4 11 | Q2([2 3 5 6],:)=-Q2([2 3 5 6],:); 12 | otherwise 13 | error('Value of kFlip invalid') 14 | end 15 | 16 | Q11=Q1(1:3,:); 17 | Q12=Q1(4:6,:); 18 | Q21=Q2(1:3,:); 19 | Q22=Q2(4:6,:); 20 | 21 | Q211=Q21*Q11'; 22 | Q212=Q22*Q12'; 23 | [tMin,fMin,tBreak1,tBreak2,tMinAll]=essential_distMinAnglePair_base(Q211,Q212); 24 | -------------------------------------------------------------------------------- /MATLAB/paper_plots/utils/get_suboptimality_gaps_from_results_fpaths.m: -------------------------------------------------------------------------------- 1 | function [abs_gaps, rel_gaps] = get_suboptimality_gaps_from_results_fpaths(results_fpaths_pairs) 2 | 3 | [num_pairs, ~] = size(results_fpaths_pairs); 4 | abs_gaps = zeros(num_pairs, 1); 5 | rel_gaps = zeros(num_pairs, 1); 6 | 7 | for i = 1:num_pairs 8 | results_fpath = results_fpaths_pairs{i, 1}; 9 | results = load(results_fpath).results; 10 | Fval_final = results.Fval_final; 11 | Fval_lifted = results.Fval_lifted; 12 | abs_gap = Fval_final - Fval_lifted; 13 | rel_gap = abs_gap / Fval_lifted; 14 | abs_gaps(i) = abs_gap; 15 | rel_gaps(i) = rel_gap; 16 | end 17 | 18 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetDirectionalDerivative.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetDirectionalDerivative(problem) 2 | % Checks whether dir. derivatives can be computed for a problem structure. 3 | % 4 | % function candoit = canGetDirectionalDerivative(problem) 5 | % 6 | % Returns true if the directional derivatives of the cost function can be 7 | % computed given the problem description, false otherwise. 8 | % 9 | % See also: canGetCost canGetGradient canGetHessian 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, Dec. 30, 2012. 13 | % Contributors: 14 | % Change log: 15 | 16 | candoit = isfield(problem, 'diff') || canGetGradient(problem); 17 | 18 | end 19 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/linearsystem/check_precond_laplace.m: -------------------------------------------------------------------------------- 1 | % TTeMPS Toolbox. 2 | % Michael Steinlechner, 2013-2016 3 | % Questions and contact: michael.steinlechner@epfl.ch 4 | % BSD 2-clause license, see LICENSE.txt 5 | 6 | function check_precond_laplace(P, grad, P_grad) 7 | 8 | d = grad.order; 9 | 10 | 11 | % first check whether P_grad is correctly gauged 12 | for ii=1:d-1 13 | orth_err(ii) = norm( unfold(P_grad.dU{ii},'left')'*unfold(grad.U{ii},'left') , 2 ) / norm( unfold(P_grad.dU{ii},'left'), 2 ); 14 | end 15 | 16 | if max(orth_err) > 1e-12 17 | orth_err 18 | warning('precond_laplace.m returned an inaccurate result') 19 | end 20 | 21 | end 22 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multisym.m: -------------------------------------------------------------------------------- 1 | function Y = multisym(X) 2 | % Returns the symmetric parts of the matrices in a 3D array 3 | % 4 | % function Y = multisym(X) 5 | % 6 | % Y is a 3D array the same size as X. Each slice Y(:, :, i) is the 7 | % symmetric part of the slice X(:, :, i), that is, 8 | % 9 | % Y(:, :, i) = .5*(X(:, :, i) + X(:, :, i).') 10 | % 11 | % Note that we do not take complex conjugates. For this, see multiherm. 12 | % 13 | % See also: multiherm multiskew multiskewh multiprod multitransp multiscale 14 | 15 | % This file is part of Manopt: www.manopt.org. 16 | % Original author: Nicolas Boumal, Jan. 31, 2013. 17 | % Contributors: 18 | % Change log: 19 | 20 | Y = .5*(X + multitransp(X)); 21 | 22 | end 23 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op_laplace/mtimes.m: -------------------------------------------------------------------------------- 1 | function A = mtimes( B, A ) 2 | %MINUS Multiplication of TT/MPS Laplace operator by scalar. 3 | % A = MTIMES(B, A) multiplies the TT/MPS Laplace operator A by the scalar B. 4 | % 5 | % See also PLUS. 6 | 7 | % TTeMPS Toolbox. 8 | % Michael Steinlechner, 2013-2016 9 | % Questions and contact: michael.steinlechner@epfl.ch 10 | % BSD 2-clause license, see LICENSE.txt 11 | 12 | %A.U{1} = B*A.U{1}; 13 | 14 | %A.U{1} = B*A.U{1}; 15 | %Numerically more stable way: distribute scalar over all cores: 16 | b = B^(1/A.order); 17 | for i = 1:A.order 18 | A.U{i} = b*A.U{i}; 19 | end 20 | end 21 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multitrace.m: -------------------------------------------------------------------------------- 1 | function tr = multitrace(A) 2 | % Computes the traces of the 2D slices in a 3D matrix. 3 | % 4 | % function tr = multitrace(A) 5 | % 6 | % For a 3-dimensional matrix A of size n-by-n-by-N, returns a column vector 7 | % tr of length N such that tr(k) = trace(A(:, :, k)); 8 | % 9 | % See also: multiprod multitransp multiscale 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, Dec. 30, 2012. 13 | % Contributors: 14 | % Change log: 15 | 16 | 17 | assert(ndims(A) <= 3, ... 18 | ['multitrace is only well defined for matrix arrays of 3 ' ... 19 | 'or fewer dimensions.']); 20 | 21 | tr = diagsum(A, 1, 2); 22 | 23 | end 24 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/essential_costE2cost.m: -------------------------------------------------------------------------------- 1 | function val = essential_costE2cost(X, costE) 2 | % Cost evaluation at X given function handle in the Essential matrix E. 3 | % 4 | % function val = essential_costE2cost(X, costE) 5 | % 6 | % costE is the function handle for the cost function in E. 7 | % 8 | % See also: essential_egradE2egrad essential_ehessE2ehess 9 | 10 | % This file is part of Manopt: www.manopt.org. 11 | % Original author: Roberto Tron, Aug. 8, 2014 12 | % Contributors: Bamdev Mishra, May 22, 2015. 13 | 14 | e3hat = [0 -1 0; 1 0 0; 0 0 0]; 15 | 16 | RA = X(:,1:3,:); 17 | RB = X(:,4:6,:); 18 | E = multiprod(multiprod(multitransp(RA), e3hat), RB); 19 | 20 | val = costE(E); 21 | end 22 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op/TTeMPS_op_to_TTeMPS.m: -------------------------------------------------------------------------------- 1 | function B = TTeMPS_op_to_TTeMPS( A ) 2 | %TTeMPS_op_to_TTeMPS Convert to TT Toolbox matrix format. 3 | % TT = TTeMPS_op_to_TTeMPS( A ) takes the TTeMPS operator A and converts it into 4 | % a TTeMPS object by reshaping. 5 | % 6 | % See also TTeMPS_op_to_TT_matrix 7 | 8 | % TTeMPS Toolbox. 9 | % Michael Steinlechner, 2013-2016 10 | % Questions and contact: michael.steinlechner@epfl.ch 11 | % BSD 2-clause license, see LICENSE.txt 12 | 13 | U = cellfun(@(y) reshape(y, [size(y,1), size(y,2)*size(y,3), size(y,4)]), ... 14 | A.U, 'UniformOutput', false); 15 | 16 | B = TTeMPS( U ); 17 | end 18 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/privateessential/essential_distMinAnglePair_ft.m: -------------------------------------------------------------------------------- 1 | %Evaluate cost function for closest representative search given coefficients 2 | %function ft=essential_distMinAnglePair_ft(t,m1,p1,c1,m2,p2,c2) 3 | %Evaluates the cost function used by essential_distMinAnglePair to find the 4 | %closest representative in the equivalence class of a QREM 5 | %If m2,p2,c2 are omitted or empty, get value of a single term 6 | function ft=essential_distMinAnglePair_ft(t,m1,p1,c1,m2,p2,c2) 7 | flagSingleTerm=false; 8 | if ~exist('m2','var') || isempty(m2) 9 | flagSingleTerm=true; 10 | end 11 | 12 | if flagSingleTerm 13 | ft=acos((m1*sin(t+p1)+c1-1)/2)^2; 14 | else 15 | ft=acos((m1*sin(t+p1)+c1-1)/2)^2+acos((m2*sin(t+p2)+c2-1)/2)^2; 16 | end -------------------------------------------------------------------------------- /MATLAB/utils/io_utils/save_experiment_results.m: -------------------------------------------------------------------------------- 1 | function save_experiment_results(exp_data_dir, results, cora_iterates_info) 2 | assert (nargin >=2, "Must have at least 2 arguments"); 3 | 4 | if ~exist(exp_data_dir, 'dir') 5 | mkdir(exp_data_dir); 6 | end 7 | 8 | % results is a struct - save it to a mat file 9 | result_path = exp_data_dir + "/factor_graph_results.mat"; 10 | save(result_path, 'results'); 11 | fprintf('Results saved to %s\n', result_path); 12 | 13 | if (nargin == 3) 14 | cora_iterates_info_path = exp_data_dir + "/factor_graph_cora_iterates_info.mat"; 15 | save(cora_iterates_info_path, 'cora_iterates_info'); 16 | fprintf('CORA iterates info saved to %s\n', cora_iterates_info_path); 17 | end 18 | end 19 | 20 | -------------------------------------------------------------------------------- /MATLAB/utils/preconditioners/get_regularized_cholesky_preconditioner.m: -------------------------------------------------------------------------------- 1 | function precon_function = get_regularized_cholesky_preconditioner(Q, condition_number_ub) 2 | [~, neg_greatest_eigval] = get_saddle_escape_direction(-Q); 3 | if (neg_greatest_eigval > 0) 4 | error('Failed to compute the greatest eigenvalue of the data matrix Q, should be negative') 5 | end 6 | lambda_reg = -neg_greatest_eigval / (condition_number_ub - 1); 7 | 8 | % https://www.mathworks.com/help/matlab/ref/ichol.html#mw_b1e10f55-a0c7-4d70-aedf-32ce0761ac96 9 | Q_width = size(Q, 1); 10 | ichol_opts.diagcomp = lambda_reg; 11 | L = ichol(Q(1:Q_width-1, 1:Q_width-1), ichol_opts); 12 | LT = L'; 13 | precon_function = @(x) [LT \ (L \ x(1:Q_width-1, :)); zeros(1, size(x, 2))]; 14 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetEuclideanGradient.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetEuclideanGradient(problem) 2 | % Checks whether the Euclidean gradient can be computed for a problem. 3 | % 4 | % function candoit = canGetEuclideanGradient(problem) 5 | % 6 | % Returns true if the Euclidean gradient can be computed given the problem 7 | % description, false otherwise. 8 | % 9 | % See also: canGetGradient getEuclideanGradient 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, Dec. 30, 2012. 13 | % Contributors: 14 | % Change log: 15 | % 16 | % June 28, 2016 (NB): 17 | % Added support for getPartialEuclideanGradient 18 | 19 | 20 | candoit = isfield(problem, 'egrad') || canGetPartialEuclideanGradient(problem); 21 | 22 | end 23 | -------------------------------------------------------------------------------- /MATLAB/manopt/examples/run_all_examples.m: -------------------------------------------------------------------------------- 1 | clear all; %#ok 2 | close all; 3 | clc; 4 | 5 | list = dir('*.m'); 6 | 7 | issues = {}; 8 | 9 | for k = 1 : numel(list) 10 | s = list(k).name; 11 | s = s(1:end-2); % remove '.m' 12 | switch s 13 | case mfilename() % don't run yourself 14 | case 'positive_definite_karcher_mean' % old name of another example 15 | otherwise 16 | try 17 | eval([s, ';']); 18 | catch 19 | issues{end+1} = s; %#ok 20 | end 21 | end 22 | end 23 | 24 | if ~isempty(issues) 25 | warning('There were issues running the following scripts:'); 26 | disp(issues); 27 | else 28 | fprintf('\n\nNo particular issues detected.\n'); 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/creal.m: -------------------------------------------------------------------------------- 1 | function Xreal = creal(X) 2 | % Extracts the real part of x 3 | % 4 | % function Xreal = creal(X) 5 | % 6 | % Returns the real part of x. The input can be either a numeric array 7 | % or a struct with fields real and imag. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July. 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if iscstruct(X) 17 | Xreal = X.real; 18 | elseif isnumeric(X) 19 | Xreal = real(X); 20 | else 21 | ME = MException('creal:inputError', ... 22 | 'Input does not have the expected format.'); 23 | throw(ME); 24 | end 25 | 26 | end 27 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/rotations/randherm.m: -------------------------------------------------------------------------------- 1 | function S = randherm(n, N) 2 | % Generates random Hermitian matrices with normal entries. 3 | % 4 | % function S = randherm(n) 5 | % function S = randherm(n, N) 6 | % 7 | % S is an n-by-n-by-N array where each slice S(:, :, i) for i = 1..N is a 8 | % random Hermitian matrix formed by 9 | % 10 | % S = (randsym(n, N) + 1i*randskew(n, N))/sqrt(2); 11 | % 12 | % By default, N = 1. 13 | % 14 | % See also: randskew randunitary randrot randsym randskewh 15 | 16 | % This file is part of Manopt: www.manopt.org. 17 | % Original author: Nicolas Boumal, June 18, 2019. 18 | % Contributors: 19 | % Change log: 20 | 21 | if nargin < 2 22 | N = 1; 23 | end 24 | 25 | S = (randsym(n, N) + 1i*randskew(n, N))/sqrt(2); 26 | 27 | end 28 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/TTeMPS_randn.m: -------------------------------------------------------------------------------- 1 | function x = TTeMPS_randn(r, n) 2 | % TTEMPS_RANDN Create random TTeMPS tensor 3 | % X = TTEMPS_RANDN(R, N) creates a length(N)-dimensional TTeMPS tensor 4 | % of size N(1)*N(2)*...N(end) with ranks R by filling the cores with 5 | % iid standard Gaussian random numbers. 6 | % Note that the first and last entry of the rank vector must be 1. 7 | % 8 | 9 | % This is a trivial modification of TTeMPS_rand, part of TTeMPS. 10 | % 11 | % See also TTeMPS_rand 12 | 13 | if length(r) ~= length(n)+1 14 | error('Size mismatch in arguments') 15 | end 16 | 17 | U = cell(1, length(n)); 18 | for i = 1:length(n) 19 | U{i} = randn(r(i), n(i), r(i+1)); 20 | end 21 | x = TTeMPS(U); 22 | end 23 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/rotations/randskewh.m: -------------------------------------------------------------------------------- 1 | function S = randskewh(n, N) 2 | % Generates random skew Hermitian matrices with normal entries. 3 | % 4 | % function S = randskewh(n) 5 | % function S = randskewh(n, N) 6 | % 7 | % S is an n-by-n-by-N array where each slice S(:, :, i) for i = 1..N is a 8 | % random skew-Hermitian matrix formed by 9 | % 10 | % S = (randskew(n, N) + 1i*randsym(n, N))/sqrt(2); 11 | % 12 | % By default, N = 1. 13 | % 14 | % See also: randherm randskew randunitary randrot randsym 15 | 16 | % This file is part of Manopt: www.manopt.org. 17 | % Original author: Nicolas Boumal, June 18, 2019. 18 | % Contributors: 19 | % Change log: 20 | 21 | if nargin < 2 22 | N = 1; 23 | end 24 | 25 | S = (randskew(n, N) + 1i*randsym(n, N))/sqrt(2); 26 | 27 | end 28 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op_laplace/TTeMPS_op_laplace_to_TT_matrix.m: -------------------------------------------------------------------------------- 1 | function B = TTeMPS_op_laplace_to_TT_matrix( A ) 2 | %TTeMPS_to_TT Convert to TT Toolbox matrix format. 3 | % TT = TT_to_TTeMPS( A ) takes the TTeMPS Laplace operator A and converts it into 4 | % a tt_matrix object using the TT Toolbox 2.x from Oseledets et al. 5 | % This toolbox needs to be installed, of course. 6 | % 7 | % See also TTeMPS_to_TT, TTeMPS_op_to_TT, TTeMPS_op_laplace_to_TTeMPS_op. 8 | 9 | % TTeMPS Toolbox. 10 | % Michael Steinlechner, 2013-2016 11 | % Questions and contact: michael.steinlechner@epfl.ch 12 | % BSD 2-clause license, see LICENSE.txt 13 | 14 | B = TTeMPS_op_to_TT_matrix( TTeMPS_op_laplace_to_TTeMPS_op( A )); 15 | 16 | end 17 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/norm.m: -------------------------------------------------------------------------------- 1 | function res = norm( x ) 2 | %NORM Norm of a TT/MPS block-mu tensor. 3 | % norm(X) computes the Frobenius norm of the TTeMPS_block tensor X. 4 | % for each of the individual vectors 1 ... X.p seperately. 5 | % Result is a column vector of norms with length X.p 6 | % 7 | % See also INNERPROD 8 | 9 | % TTeMPS Toolbox. 10 | % Michael Steinlechner, 2013-2016 11 | % Questions and contact: michael.steinlechner@epfl.ch 12 | % BSD 2-clause license, see LICENSE.txt 13 | 14 | x = orthogonalize( x ); 15 | block = x.U{x.mu}; 16 | 17 | res = zeros(x.p,1); 18 | for i = 1:x.p 19 | tmp = block(:,:,:,i); 20 | res(i) = norm( tmp(:) ); 21 | end 22 | 23 | end 24 | -------------------------------------------------------------------------------- /MATLAB/paper_plots/utils/get_num_poses_from_fpath_names.m: -------------------------------------------------------------------------------- 1 | function num_poses = get_num_poses_from_fpath_names(results_fpaths_pairs) 2 | [num_pairs, ~] = size(results_fpaths_pairs); 3 | num_poses = zeros(num_pairs, 1); 4 | for i = 1:num_pairs 5 | [~, name, ~] = fileparts(results_fpaths_pairs{i, 1}); 6 | 7 | % example file: 8 | % factor_graph_4robots_0.5rangeStddev_9500poses_500ranges_0loopClosures_29997seed_results.mat 9 | % we want to extract the number of poses - 9500 in this case 10 | leading_str = "Stddev_"; 11 | trailing_str = "poses_"; 12 | start_idx = strfind(name, leading_str) + strlength(leading_str); 13 | end_idx = strfind(name, trailing_str) - 1; 14 | char_name = char(name); 15 | num_poses(i) = str2double(char_name(start_idx:end_idx)); 16 | end 17 | end -------------------------------------------------------------------------------- /MATLAB/paper_plots/utils/get_num_ranges_from_fpath_names.m: -------------------------------------------------------------------------------- 1 | function num_ranges = get_num_ranges_from_fpath_names(results_fpaths_pairs) 2 | [num_pairs, ~] = size(results_fpaths_pairs); 3 | num_ranges = zeros(num_pairs, 1); 4 | for i = 1:num_pairs 5 | [~, name, ~] = fileparts(results_fpaths_pairs{i, 1}); 6 | 7 | % example file: 8 | % factor_graph_4robots_0.5rangeStddev_9500poses_500ranges_0loopClosures_29997seed_results.mat 9 | % we want to extract the number of poses - 9500 in this case 10 | leading_str = "poses_"; 11 | trailing_str = "ranges_"; 12 | start_idx = strfind(name, leading_str) + strlength(leading_str); 13 | end_idx = strfind(name, trailing_str) - 1; 14 | char_name = char(name); 15 | num_ranges(i) = str2double(char_name(start_idx:end_idx)); 16 | end 17 | end -------------------------------------------------------------------------------- /MATLAB/paper_plots/utils/get_num_robots_from_fpath_names.m: -------------------------------------------------------------------------------- 1 | function num_robots = get_num_robots_from_fpath_names(results_fpaths_pairs) 2 | [num_pairs, ~] = size(results_fpaths_pairs); 3 | num_robots = zeros(num_pairs, 1); 4 | for i = 1:num_pairs 5 | [~, name, ~] = fileparts(results_fpaths_pairs{i, 1}); 6 | 7 | % example file: 8 | % factor_graph_4robots_0.5rangeStddev_9500poses_500ranges_0loopClosures_29997seed_results.mat 9 | % we want to extract the number of poses - 9500 in this case 10 | leading_str = "graph_"; 11 | trailing_str = "robots_"; 12 | start_idx = strfind(name, leading_str) + strlength(leading_str); 13 | end_idx = strfind(name, trailing_str) - 1; 14 | char_name = char(name); 15 | num_robots(i) = str2double(char_name(start_idx:end_idx)); 16 | end 17 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/cconj.m: -------------------------------------------------------------------------------- 1 | function xconj = cconj(x) 2 | % Computes the complex conjugate of x 3 | % 4 | % function xconj = cconj(x) 5 | % 6 | % Returns the complex conjugate of x. The input can be either a numeric 7 | % array or a struct with fields real and imag. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July. 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if iscstruct(x) 17 | xconj.real = x.real; 18 | xconj.imag = -x.imag; 19 | elseif isnumeric(x) 20 | xconj = conj(x); 21 | else 22 | ME = MException('cconj:inputError', ... 23 | 'Input does not have the expected format.'); 24 | throw(ME); 25 | end 26 | 27 | end 28 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/trunc_singular.m: -------------------------------------------------------------------------------- 1 | function s = trunc_singular(s, tol, relative, maxrank) 2 | % REL_TRUNC_SINGULAR Helper routine to truncate singular values 3 | 4 | % TTeMPS Toolbox. 5 | % Michael Steinlechner, 2013-2016 6 | % Questions and contact: michael.steinlechner@epfl.ch 7 | % BSD 2-clause license, see LICENSE.txt 8 | 9 | if ~exist('relative','var'), relative = true; end 10 | if ~exist('maxrank','var'), maxrank = length(s); end 11 | 12 | summ = cumsum(s.^2,'reverse'); 13 | 14 | if relative 15 | s = find(summ > tol^2, 1, 'last'); 16 | if isempty(s), s = 1; end 17 | else 18 | s = find(summ > tol^2*summ(1), 1, 'last'); 19 | if isempty(s), s = 1; end 20 | end 21 | 22 | s = min([s, maxrank, length(s)]); 23 | 24 | end 25 | 26 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/TTeMPS_rand.m: -------------------------------------------------------------------------------- 1 | function x = TTeMPS_rand(r, n) 2 | % TTEMPS_RAND Create random TTeMPS tensor 3 | % X = TTEMPS_RAND(R, N) creates a length(N)-dimensional TTeMPS tensor 4 | % of size N(1)*N(2)*...N(end) with ranks R by filling the cores with 5 | % iid uniform random numbers in [0, 1]. 6 | % Note that the first and last entry of the rank vector must be 1. 7 | % 8 | 9 | % TTeMPS Toolbox. 10 | % Michael Steinlechner, 2013-2016 11 | % Questions and contact: michael.steinlechner@epfl.ch 12 | % BSD 2-clause license, see LICENSE.txt 13 | 14 | if length(r) ~= length(n)+1 15 | error('Size mismatch in arguments') 16 | end 17 | 18 | U = cell(1, length(n)); 19 | for i = 1:length(n) 20 | U{i} = rand(r(i), n(i), r(i+1)); 21 | end 22 | x = TTeMPS(U); 23 | end 24 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/orthogonalizetwice.m: -------------------------------------------------------------------------------- 1 | function [Q, R] = orthogonalizetwice(M, x, A) 2 | % Orthonormalizes a basis of tangent vectors twice for increased accuracy. 3 | % 4 | % function [orthobasis, R] = orthogonalizetwice(M, x, basis) 5 | % 6 | % See help for orthogonalize. This function calls that algorithm twice. 7 | % This is useful if elements in the input basis are close to being linearly 8 | % dependent (ill conditioned). See in code for details. 9 | % 10 | % See also: orthogonalize grammatrix tangentorthobasis 11 | 12 | % This file is part of Manopt: www.manopt.org. 13 | % Original author: Nicolas Boumal, Oct. 5, 2017. 14 | % Contributors: 15 | % Change log: 16 | 17 | [Q1, R1] = orthogonalize(M, x, A); 18 | [Q , R2] = orthogonalize(M, x, Q1); 19 | 20 | R = R2*R1; % This is upper triangular since R1 and R2 are. 21 | 22 | end 23 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op_laplace/TTeMPS_op_laplace_to_TTeMPS_op.m: -------------------------------------------------------------------------------- 1 | function res = TTeMPS_op_laplace_to_TTeMPS_op( A ) 2 | %TTeMPS_op_laplace_to_TTeMPS Convert to TT_op_laplace to TTeMPS_op format. 3 | % 4 | % See also TTeMPS_to_TT, TTeMPS_op_to_TT, TTeMPS_op_laplace_to_TTeMPS_op. 5 | 6 | % TTeMPS Toolbox. 7 | % Michael Steinlechner, 2013-2016 8 | % Questions and contact: michael.steinlechner@epfl.ch 9 | % BSD 2-clause license, see LICENSE.txt 10 | 11 | C = cell(1, A.order); 12 | for i = 1:A.order 13 | % make a 4D tensor out of it again 14 | tmp = reshape( full(A.U{i}), [A.rank(i), A.rank(i+1), A.size_col(i), A.size_row(i)] ); 15 | % inverse permute the indices 16 | C{i} = ipermute( tmp, [1 4 2 3] ); 17 | end 18 | res = TTeMPS_op(C); 19 | end 20 | -------------------------------------------------------------------------------- /MATLAB/utils/misc_utils/get_robot_R_idxs.m: -------------------------------------------------------------------------------- 1 | function R_idxs = get_robot_R_idxs(problem_data, robot_idx) 2 | 3 | % has_priors = (problem_data.num_pose_priors + problem_data.num_landmark_priors) > 0; 4 | warning("Assuming no priors - get_robot_R_idxs") 5 | has_priors = 0; 6 | 7 | num_robot_poses = problem_data.num_poses-has_priors; %remove the aux pose 8 | num_robots = problem_data.num_robots; 9 | poses_per_robot = num_robot_poses / num_robots; 10 | 11 | % skip the aux pose 12 | all_robot_start_idx = (problem_data.dim * has_priors)+1; 13 | per_robot_rot_block_span = 1:problem_data.dim * poses_per_robot; 14 | 15 | cur_robot_rot_block_start = (robot_idx-1) * problem_data.dim * poses_per_robot; 16 | all_robot_R_idxs = problem_data.all_R_idxs(all_robot_start_idx:end); 17 | R_idxs = all_robot_R_idxs(cur_robot_rot_block_start + per_robot_rot_block_span); 18 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetPrecon.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetPrecon(problem) 2 | % Checks whether a preconditioner was specified in the problem description. 3 | % 4 | % function candoit = canGetPrecon(problem) 5 | % 6 | % Returns true if a preconditioner was specified, false otherwise. Notice 7 | % that even if this function returns false, it is still possible to call 8 | % getPrecon, as the default preconditioner is simply the identity operator. 9 | % This check function is mostly useful to tell whether that default 10 | % preconditioner will be in use or not. 11 | % 12 | % See also: getPrecon getSqrtPrecon canGetSqrtPrecon getHessian 13 | 14 | % This file is part of Manopt: www.manopt.org. 15 | % Original author: Nicolas Boumal, July 3, 2013. 16 | % Contributors: 17 | % Change log: 18 | 19 | candoit = isfield(problem, 'precon') || canGetSqrtPrecon(problem); 20 | 21 | end 22 | -------------------------------------------------------------------------------- /MATLAB/paper_plots/utils/get_range_covs_from_fpath_names.m: -------------------------------------------------------------------------------- 1 | function range_covs = get_range_covs_from_fpath_names(results_fpaths_pairs) 2 | [num_pairs, ~] = size(results_fpaths_pairs); 3 | range_stddevs = zeros(num_pairs, 1); 4 | for i = 1:num_pairs 5 | [~, name, ~] = fileparts(results_fpaths_pairs{i, 1}); 6 | 7 | % example file: 8 | % factor_graph_4robots_0.5rangeStddev_9500poses_500ranges_0loopClosures_29997seed_results.mat 9 | % we want to extract the number of poses - 9500 in this case 10 | leading_str = "robots_"; 11 | trailing_str = "rangeStddev_"; 12 | start_idx = strfind(name, leading_str) + strlength(leading_str); 13 | end_idx = strfind(name, trailing_str) - 1; 14 | char_name = char(name); 15 | range_stddevs(i) = str2double(char_name(start_idx:end_idx)); 16 | end 17 | range_covs = range_stddevs.^2; 18 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/cmat2col.m: -------------------------------------------------------------------------------- 1 | function Xcol = cmat2col(X) 2 | % Converts X into a column vector 3 | % 4 | % function Xcol = cmat2col(X) 5 | % 6 | % Returns X(:) where the input X can be either a numeric array or a 7 | % struct with fields real and imag. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July. 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if iscstruct(X) 17 | Xreal = X.real; 18 | Ximag = X.imag; 19 | Xcol.real = Xreal(:); 20 | Xcol.imag = Ximag(:); 21 | elseif isnumeric(X) 22 | Xcol = X(:); 23 | else 24 | ME = MException('cmat2col:inputError', ... 25 | 'Input does not have the expected format.'); 26 | throw(ME); 27 | end 28 | 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/TT_to_TTeMPS.m: -------------------------------------------------------------------------------- 1 | function x = TT_to_TTeMPS( tt ) 2 | %TT_to_TTeMPS Convert from TT Toolbox format. 3 | % A = TT_to_TTeMPS( tt ) takes the tt_tensor object tt created using the 4 | % TT Toolbox 2.x from Oseledets et al. and converts it into a TTeMPS tensor. 5 | % Toolbox needs to be installed, of course. 6 | % 7 | % See also TTeMPS_to_TT. 8 | 9 | % TTeMPS Toolbox. 10 | % Michael Steinlechner, 2013-2016 11 | % Questions and contact: michael.steinlechner@epfl.ch 12 | % BSD 2-clause license, see LICENSE.txt 13 | 14 | cores = {}; 15 | ps = tt.ps; 16 | for i = 1:tt.d 17 | cores{i} = reshape( tt.core( ps(i):ps(i+1)-1 ), ... 18 | [tt.r(i), tt.n(i), tt.r(i+1)] ); 19 | end 20 | 21 | x = TTeMPS( cores ); 22 | 23 | end 24 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multisqnorm.m: -------------------------------------------------------------------------------- 1 | function sqnorm = multisqnorm(A) 2 | % Returns the squared Frobenius norms of the slices of a 3D matrix. 3 | % 4 | % function sqnorm = multisqnorm(A) 5 | % 6 | % Given a 3-dimensional matrix A of size n-by-m-by-N, returns a column 7 | % vector of length N such that sqnorm(i) = norm(A(:, :, i), 'fro')^2. 8 | % 9 | % See also: multiprod multitransp multitrace norms 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, June 17, 2015. 13 | % Contributors: 14 | % Change log: 15 | 16 | 17 | assert(ndims(A) <= 3, ... 18 | ['multisqnorm is only well defined for matrix arrays of 3 ' ... 19 | 'or less dimensions.']); 20 | [n, m, N] = size(A); 21 | 22 | % This is equivalent to squeeze(sum(norms(A, 2, 1).^2)), but faster. 23 | sqnorm = sum(reshape(A, n*m, N).^2, 1)'; 24 | 25 | end 26 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op/round.m: -------------------------------------------------------------------------------- 1 | function A = round(A, tol ) 2 | %ROUND Approximate TTeMPS operator within a prescribed tolerance. 3 | % X = ROUND( A, tol ) truncates the given TTeMPS operator A to a 4 | % lower rank such that the error is in order of tol. 5 | 6 | % TTeMPS Toolbox. 7 | % Michael Steinlechner, 2013-2016 8 | % Questions and contact: michael.steinlechner@epfl.ch 9 | % BSD 2-clause license, see LICENSE.txt 10 | 11 | C = cell(1, A.order); 12 | for i = 1:A.order 13 | C{i} = reshape(A.U{i}, [A.rank(i), A.size_col(i)*A.size_row(i), A.rank(i+1)]); 14 | end 15 | X = TTeMPS( C ); 16 | X = round(X, tol); 17 | for i = 1:A.order 18 | A.U{i} = reshape(X.U{i}, [X.rank(i), A.size_col(i), A.size_row(i), X.rank(i+1)]); 19 | end 20 | 21 | A = update_properties(A); 22 | 23 | end 24 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/ctransp.m: -------------------------------------------------------------------------------- 1 | function Xtransp = ctransp(X) 2 | % Computes the conjugate-transpose of X 3 | % 4 | % function Xtransp = ctransp(X) 5 | % 6 | % This function can be seen as the operator ' but is compatible with 7 | % both dlarrays and structs with fields real and iamg. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July. 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if iscstruct(X) 17 | Xreal = X.real; 18 | Ximag = X.imag; 19 | Xtransp.real = Xreal'; 20 | Xtransp.imag = -Ximag'; 21 | elseif isnumeric(X) 22 | Xtransp = X'; 23 | else 24 | ME = MException('ctransp:inputError', ... 25 | 'Input does not have the expected format.'); 26 | throw(ME); 27 | end 28 | 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/full.m: -------------------------------------------------------------------------------- 1 | function y = full( x ) 2 | %FULL Convert TTeMPS tensor to full array 3 | % X = FULL(X) converts the TTeMPS tensor X to a (X.order)-dimensional full array of size 4 | % X.size(1) x X.size(2) x ... x X.size(X.order) 5 | % 6 | % Use with care! Result can easily exceed available memory. 7 | % 8 | % See also SUBSREF. 9 | 10 | % TTeMPS Toolbox. 11 | % Michael Steinlechner, 2013-2016 12 | % Questions and contact: michael.steinlechner@epfl.ch 13 | % BSD 2-clause license, see LICENSE.txt 14 | 15 | 16 | y = x.U{1}; 17 | y = reshape( y, [ x.size(1), x.rank(2) ]); 18 | for i = 2:x.order 19 | U_temp = unfold( x.U{i}, 'right'); 20 | y = y * U_temp; 21 | y = reshape( y, [ prod(x.size(1:i)), x.rank(i+1) ]); 22 | end 23 | y = reshape( y, x.size ); 24 | end 25 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op/full.m: -------------------------------------------------------------------------------- 1 | function Afull = full( A ) 2 | %FULL Convert TTeMPS_op operator to full array 3 | % X = FULL(A) converts the TTeMPS_op operator A to an (A.order)-dimensional full array. 4 | % 5 | % Use with care! Result can easily exceed available memory. 6 | % 7 | 8 | % TTeMPS Toolbox. 9 | % Michael Steinlechner, 2013-2016 10 | % Questions and contact: michael.steinlechner@epfl.ch 11 | % BSD 2-clause license, see LICENSE.txt 12 | 13 | d = A.order; 14 | X = full( TTeMPS_op_to_TTeMPS(A) ); 15 | permutevec = [1:d; d+1:2*d]; 16 | permutevec = permutevec(:)'; 17 | 18 | sizes = [A.size_col, A.size_row]; 19 | sizes = sizes( permutevec ); 20 | X = reshape( X, sizes(permutevec) ); 21 | X = ipermute( X, permutevec ); 22 | Afull = reshape( X, prod(A.size_col), prod(A.size_row) ); 23 | end 24 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/full.m: -------------------------------------------------------------------------------- 1 | function y = full( x ) 2 | %FULL Convert TTeMPS tensor to full array 3 | % X = FULL(X) converts the TTeMPS tensor X to a (X.order)-dimensional full array of size 4 | % X.size(1) x X.size(2) x ... x X.size(X.order) 5 | % 6 | % Use with care! Result can easily exceed available memory. 7 | % 8 | % See also SUBSREF. 9 | 10 | % TTeMPS Toolbox. 11 | % Michael Steinlechner, 2013-2016 12 | % Questions and contact: michael.steinlechner@epfl.ch 13 | % BSD 2-clause license, see LICENSE.txt 14 | r = x.rank; 15 | n = x.size; 16 | 17 | y = x.U{1}; 18 | y = reshape( y, [n(1), r(2) ]); 19 | for i = 2:x.order 20 | U_temp = unfold( x.U{i}, 'right'); 21 | y = y * U_temp; 22 | y = reshape( y, [ prod(n(1:i)), r(i+1) ]); 23 | end 24 | y = reshape( y, n ); 25 | end 26 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/creshape.m: -------------------------------------------------------------------------------- 1 | function Xreshape = creshape(X, varargin) 2 | % Reshapes X. 3 | % 4 | % function Xreshape = creshape(X,varargin) 5 | % 6 | % This function can be seen as reshape(X,varargin) but is compatible with 7 | % structs with fields real and imag. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July. 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if iscstruct(X) 17 | Xreshape.real = reshape(X.real, varargin{:}); 18 | Xreshape.imag = reshape(X.imag, varargin{:}); 19 | 20 | elseif isnumeric(X) 21 | Xreshape = reshape(X, varargin{:}); 22 | 23 | else 24 | ME = MException('creshape:inputError', ... 25 | 'Input does not have the expected format.'); 26 | throw(ME); 27 | 28 | end 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/truncate.m: -------------------------------------------------------------------------------- 1 | function x = truncate( x, r ) 2 | %TRUNCATE Truncate TTeMPS tensor to prescribed rank. 3 | % X = TRUNCATE( X, R ) truncates the given TTeMPS tensor X to rank R. 4 | % The result is a right orthogonal TTeMPS. 5 | 6 | % TTeMPS Toolbox. 7 | % Michael Steinlechner, 2013-2016 8 | % Questions and contact: michael.steinlechner@epfl.ch 9 | % BSD 2-clause license, see LICENSE.txt 10 | 11 | 12 | % Left-right procedure 13 | x = x.orthogonalize( x.order ); 14 | 15 | for i = x.order:-1:2 16 | [U,S,V] = svd( unfold( x.U{i}, 'right'), 'econ'); 17 | s = min( r(i), length(S)); 18 | U = U(:,1:s); 19 | V = V(:,1:s); 20 | S = S(1:s,1:s); 21 | x.U{i} = reshape( V', [s, x.size(i), x.rank(i+1)] ); 22 | x.U{i-1} = tensorprod_ttemps( x.U{i-1}, (U*S)', 3 ); 23 | end 24 | 25 | end 26 | -------------------------------------------------------------------------------- /MATLAB/utils/viz_utils/align_solution_by_first_pose.m: -------------------------------------------------------------------------------- 1 | function X = align_solution_by_first_pose(X, dim, first_rot_idxs, t_idxs, l_idxs) 2 | % make sure we have 4 args 3 | assert (nargin == 5, "align_solution_by_first_pose requires 5 arguments"); 4 | assert (dim == length(first_rot_idxs), "first_rot_idxs should be a vector of indices"); 5 | 6 | [nrows, ncols] = size(X); 7 | 8 | % matrix should be short and wide 9 | assert(nrows < ncols, "There should be more columns than rows, try transposing the matrix"); 10 | 11 | % if there is a prior then the first pose is an "origin" frame and can 12 | % be used to align the solution 13 | R_origin = X(:, first_rot_idxs); 14 | t_origin = X(:, t_idxs(1)); 15 | R_inv_origin = R_origin'; 16 | t_inv_origin = - (R_inv_origin * t_origin); 17 | X = R_inv_origin * X; 18 | X(:, t_idxs) = X(:, t_idxs) + t_inv_origin; 19 | X(:, l_idxs) = X(:, l_idxs) + t_inv_origin; 20 | 21 | end 22 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/crepmat.m: -------------------------------------------------------------------------------- 1 | function Xrepmat = crepmat(X, varargin) 2 | % Replicates an array. 3 | % 4 | % function Xrepmat = crepmat(X,varargin) 5 | % 6 | % This function can be seen as repmat(X,varargin) but is compatible with 7 | % structs with fields real and imag. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July. 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if iscstruct(X) 17 | Xrepmat.real = repmat(X.real, varargin{:}); 18 | Xrepmat.imag = repmat(X.imag, varargin{:}); 19 | 20 | elseif isnumeric(X) 21 | Xrepmat = repmat(X, varargin{:}); 22 | 23 | else 24 | ME = MException('crepmat:inputError', ... 25 | 'Input does not have the expected format.'); 26 | throw(ME); 27 | 28 | end 29 | 30 | end 31 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/TTeMPS_to_TT.m: -------------------------------------------------------------------------------- 1 | function TT = TTeMPS_to_TT( x ) 2 | %TTeMPS_to_TT Convert to TT Toolbox format. 3 | % TT = TT_to_TTeMPS( A ) takes the TTeMPS tensor A and converts it into 4 | % a tt_tensor object tt using the TT Toolbox 2.x from Oseledets et al. 5 | % Toolbox needs to be installed, of course. 6 | % 7 | % See also TTeMPS_to_TT. 8 | 9 | % TTeMPS Toolbox. 10 | % Michael Steinlechner, 2013-2016 11 | % Questions and contact: michael.steinlechner@epfl.ch 12 | % BSD 2-clause license, see LICENSE.txt 13 | 14 | TT = tt_tensor; 15 | TT.d = x.order; 16 | TT.n = x.size'; 17 | TT.r = x.rank'; 18 | 19 | % cellfun goodness! 20 | TT.core = cell2mat( cellfun(@(y) y(:).', x.U, 'UniformOutput', false) ).'; 21 | lengths = TT.r(1:end-1) .* TT.n .* TT.r(2:end); 22 | TT.ps = cumsum( [1; lengths] ); 23 | 24 | end 25 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetApproxGradient.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetApproxGradient(problem) 2 | % Checks whether an approximate gradient can be computed for this problem. 3 | % 4 | % function candoit = canGetApproxGradient(problem) 5 | % 6 | % Returns true if an approximate gradient of the cost function is provided 7 | % in the given problem description, false otherwise. 8 | % If a gradient is defined but no approximate gradient is defined 9 | % explicitly, returns false. 10 | % 11 | % Even if this returns false, calls to getApproxGradient may succeed, as 12 | % they will be redirected to getGradientFD. The latter simply requires 13 | % availability of the cost in problem. 14 | % 15 | % See also: canGetGradient getGradientFD 16 | 17 | % This file is part of Manopt: www.manopt.org. 18 | % Original author: Nicolas Boumal, Nov. 1, 2016. 19 | % Contributors: 20 | % Change log: 21 | 22 | candoit = isfield(problem, 'approxgrad'); 23 | 24 | end 25 | -------------------------------------------------------------------------------- /MATLAB/utils/io_utils/get_experiment_filepaths_in_subdirs.m: -------------------------------------------------------------------------------- 1 | function file_paths = get_experiment_filepaths_in_subdirs(search_dir) 2 | 3 | % check that the directory exists 4 | if ~exist(search_dir, 'dir') 5 | error('Directory %s does not exist', search_dir); 6 | end 7 | 8 | file_substr = "factor_graph"; 9 | 10 | % recursively search the directory for files that begin with file_substr 11 | file_paths = dir(fullfile(search_dir, '**', file_substr + "*.mat")); 12 | 13 | % throw out any directories that were found 14 | file_paths = file_paths(~[file_paths.isdir]); 15 | 16 | % don't keep files that end with results.mat or _info.mat 17 | file_paths = file_paths(~endsWith({file_paths.name}, 'results.mat')); 18 | file_paths = file_paths(~endsWith({file_paths.name}, '_info.mat')); 19 | 20 | % get the full path of each file as a string 21 | file_paths = string({file_paths.folder}) + "/" + string({file_paths.name}); 22 | 23 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetApproxHessian.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetApproxHessian(problem) 2 | % Checks whether an approximate Hessian can be computed for this problem. 3 | % 4 | % function candoit = canGetApproxHessian(problem) 5 | % 6 | % Returns true if an approximate Hessian of the cost function is provided 7 | % in the given problem description, false otherwise. 8 | % If a Hessian is defined but no approximate Hessian is defined explicitly, 9 | % returns false. 10 | % 11 | % Even if this returns false, calls to getApproxHessian may succeed, as 12 | % they will be redirected to getHessianFD. The latter simply requires 13 | % availability of gradients in problem, and vector transports in problem.M. 14 | % 15 | % See also: canGetHessian getHessianFD 16 | 17 | % This file is part of Manopt: www.manopt.org. 18 | % Original author: Nicolas Boumal, April 8, 2015. 19 | % Contributors: 20 | % Change log: 21 | 22 | candoit = isfield(problem, 'approxhess'); 23 | 24 | end 25 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/unfold.m: -------------------------------------------------------------------------------- 1 | function W = unfold( U, dir ) 2 | %UNFOLD Left/right-unfold a 3D array. 3 | % W = UNFOLD(U, DIR) unfolds the 3-dim. tensor U in direction DIR, where 4 | % DIR is either 'LEFT' or 'RIGHT' (case insensitive). 5 | % 6 | % See also MATRICIZE, TENSORIZE, TENSORPROD_TTEMPS. 7 | 8 | % TTeMPS Toolbox. 9 | % Michael Steinlechner, 2013-2016 10 | % Questions and contact: michael.steinlechner@epfl.ch 11 | % BSD 2-clause license, see LICENSE.txt 12 | 13 | d = size(U); 14 | % pad with 1 for the last dim (annoying) 15 | if length(size(U)) == 2 16 | d = [d, 1]; 17 | end 18 | 19 | if strcmpi(dir, 'left') 20 | W = reshape( U, [d(1)*d(2), d(3)] ); 21 | elseif strcmpi(dir, 'right') 22 | W = reshape( U, [d(1), d(2)*d(3)] ); 23 | else 24 | error('Unknown direction specified. Choose either LEFT or RIGHT') 25 | end 26 | end 27 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/cindex.m: -------------------------------------------------------------------------------- 1 | function Xindex = cindex(X) 2 | % Array Indexing 3 | % 4 | % function Xindex = cindex(X) 5 | % 6 | % Indexing X according to the user's need. Fill the ... in the bracket. 7 | % The input should accept either a numeric array or a struct with fields 8 | % real and imag. 9 | % 10 | % See also: manoptADhelp 11 | 12 | % This file is part of Manopt: www.manopt.org. 13 | % Original author: Xiaowen Jiang, July 31, 2021. 14 | % Contributors: 15 | % Change log: 16 | 17 | fprintf(['cindex: This is a template. Please create your own ' ... 18 | 'function following the instructions in the comments.']) 19 | 20 | % if iscstruct(X) 21 | % Xindex.real = X(...); 22 | % Xindex.imag = X(...); 23 | % elseif isnumeric(X) 24 | % Xindex = X(...); 25 | % else 26 | % ME = MException('cindex:inputError', ... 27 | % 'Input does not have the expected format.'); 28 | % throw(ME); 29 | % end 30 | 31 | end 32 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/essential_egradE2egrad.m: -------------------------------------------------------------------------------- 1 | function egrad = essential_egradE2egrad(X, egradE) 2 | % Converts the gradient in essential matrix E to the gradient in X. 3 | % 4 | % function egrad = essential_egradE2egrad(X, egradE) 5 | % 6 | % egradE is the function handle for the gradient in E. 7 | % 8 | % The output is a matrix in the space of X. 9 | % 10 | % See also: essential_costE2cost essential_ehessE2ehess 11 | 12 | 13 | % This file is part of Manopt: www.manopt.org. 14 | % Original author: Roberto Tron, Aug. 8, 2014 15 | % Contributors: Bamdev Mishra, May 22, 2015. 16 | 17 | e3hat = [0 -1 0; 1 0 0; 0 0 0]; 18 | RA = X(:,1:3,:); 19 | RB = X(:,4:6,:); 20 | E = multiprod(multiprod(multitransp(RA), e3hat), RB); 21 | G = egradE(E); 22 | 23 | %The following is the vectorized version of egrad = e3hat*[RB*G' -RA*G]; 24 | egrad = multiprod(e3hat, cat(2,... 25 | multiprod(RB, multitransp(G)),... 26 | -multiprod(RA, G))); 27 | end 28 | 29 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/completion/makeOmegaSet_mod.m: -------------------------------------------------------------------------------- 1 | % TTeMPS Toolbox. 2 | % Michael Steinlechner, 2013-2016 3 | % Questions and contact: michael.steinlechner@epfl.ch 4 | % BSD 2-clause license, see LICENSE.txt 5 | function Omega = makeOmegaSet_mod( n, sizeOmega ) 6 | 7 | if sizeOmega > prod(n) 8 | error('makeOmegaSet:sizeOmegaTooHigh', 'Requested size of Omega is bigger than the tensor itself!') 9 | end 10 | 11 | d = length(n); 12 | subs = zeros(sizeOmega,d); 13 | 14 | for i = 1:d 15 | subs(:,i) = randi( n(i), sizeOmega, 1 ); 16 | end 17 | 18 | Omega = unique( subs, 'rows' ); 19 | m = size(Omega, 1); 20 | 21 | while m < sizeOmega 22 | subs(1:m,:) = Omega; 23 | for i=1:d 24 | subs(m+1:sizeOmega, i) = randi( n(i), sizeOmega-m, 1 ); 25 | end 26 | Omega = unique( subs, 'rows' ); 27 | m = size(Omega, 1); 28 | end 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op/TTeMPS_op_to_TT_matrix.m: -------------------------------------------------------------------------------- 1 | function B = TTeMPS_op_to_TT_matrix( A ) 2 | %TTeMPS_op_to_TT Convert to TT Toolbox matrix format. 3 | % TT = TT_op_to_TTeMPS( A ) takes the TTeMPS operator A and converts it into 4 | % a tt_matrix object using the TT Toolbox 2.x from Oseledets et al. 5 | % This toolbox needs to be installed, of course. 6 | % 7 | % See also TTeMPS_to_TT. 8 | 9 | % TTeMPS Toolbox. 10 | % Michael Steinlechner, 2013-2016 11 | % Questions and contact: michael.steinlechner@epfl.ch 12 | % BSD 2-clause license, see LICENSE.txt 13 | 14 | TT = tt_tensor; 15 | TT.d = A.order; 16 | TT.n = [A.size_col.*A.size_row]'; 17 | TT.r = A.rank'; 18 | TT.core = cell2mat( cellfun(@(y) y(:).', A.U, 'UniformOutput', false) ).'; 19 | lengths = TT.r(1:end-1) .* TT.n .* TT.r(2:end); 20 | TT.ps = cumsum( [1; lengths] ); 21 | 22 | B = tt_matrix( TT, A.size_col', A.size_row' ); 23 | 24 | end 25 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/norm.m: -------------------------------------------------------------------------------- 1 | function res = norm( x, safe ) 2 | %NORM Norm of a TT/MPS tensor. 3 | % norm(X) computes the Frobenius norm of the TT/MPS tensor X. 4 | % 5 | % norm(X, SAFE) with SAFE=true computes the Frobenius norm of the TT/MPS tensor X 6 | % with reorthogonalization to increase the accuracy 7 | % 8 | % See also INNERPROD 9 | 10 | % TTeMPS Toolbox. 11 | % Michael Steinlechner, 2013-2016 12 | % Questions and contact: michael.steinlechner@epfl.ch 13 | % BSD 2-clause license, see LICENSE.txt 14 | 15 | if ~exist('safe','var') 16 | safe = true; 17 | end 18 | 19 | if safe 20 | x = orthogonalize(x, x.order ); 21 | res = norm( x.U{end}(:) ); 22 | else 23 | res = sqrt(innerprod( x, x )); 24 | 25 | if res < 1e-7 26 | x = orthogonalize(x, x.order ); 27 | res = norm( x.U{end}(:) ); 28 | end 29 | end 30 | 31 | end 32 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/round.m: -------------------------------------------------------------------------------- 1 | function x = round( x, tol ) 2 | %ROUND Approximate TTeMPS tensor within a prescribed tolerance. 3 | % X = ROUND( X, tol ) truncates the given TTeMPS tensor X to a 4 | % lower rank such that the error is in order of tol. 5 | 6 | % TTeMPS Toolbox. 7 | % Michael Steinlechner, 2013-2016 8 | % Questions and contact: michael.steinlechner@epfl.ch 9 | % BSD 2-clause license, see LICENSE.txt 10 | 11 | sz = x.size; 12 | d = x.order; 13 | 14 | % Left-right procedure 15 | x = x.orthogonalize( d ); 16 | 17 | right_rank = 1; 18 | for i = d:-1:2 19 | [U,S,V] = svd( unfold( x.U{i}, 'right'), 'econ' ); 20 | r = trunc_singular( diag(S), tol, true ); 21 | U = U(:,1:r); 22 | V = V(:,1:r); 23 | S = S(1:r,1:r); 24 | x.U{i} = reshape( V', [r, sz(i), right_rank] ); 25 | x.U{i-1} = tensorprod_ttemps( x.U{i-1}, (U*S).', 3 ); 26 | right_rank = r; 27 | end 28 | 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetPartialEuclideanGradient.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetPartialEuclideanGradient(problem) 2 | % Checks whether the partial Euclidean gradient can be computed for a problem. 3 | % 4 | % function candoit = canGetPartialEuclideanGradient(problem) 5 | % 6 | % Returns true if the partial Euclidean gradient of the cost function can 7 | % be computed given the problem description, false otherwise. 8 | % 9 | % See also: getPartialEuclideanGradient canGetPartialGradient 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, June 28, 2016. 13 | % Contributors: 14 | % Change log: 15 | 16 | candoit = (isfield(problem, 'partialegrad') && isfield(problem, 'ncostterms')); 17 | 18 | if isfield(problem, 'partialegrad') && ~isfield(problem, 'ncostterms') 19 | warning('manopt:partialegrad', ... 20 | ['If problem.partialegrad is specified, indicate the number n\n' ... 21 | 'of terms in the cost function with problem.ncostterms = n.']); 22 | end 23 | 24 | end 25 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/gauge_matrices.m: -------------------------------------------------------------------------------- 1 | function [xL, xR, G] = gauge_matrices( x ) 2 | % GAUGE_MATRICES Right and left orthogonalization with storage of gauge matrices 3 | % 4 | % [xL,xR,G] = GAUGE_MATRICES(x) Compute a left and right orthogonalization and 5 | % keep the gauge matrices that relates them. 6 | % 7 | % The i-th core of xR 8 | % unfold(xR.U{i},'left') 9 | % is equal to the transformed i-th core of xL 10 | % kron(eye(n(i)),inv(G{i-1}))*unfold(xL.U{i},'left')*G{i} 11 | % (where fore i=1 and i=d, G{i} = 1). 12 | % 13 | % Or, equivalently 14 | % tensorprod_ttemps( tensorprod_ttemps( xL.U{i}, G{i}', 3), inv(G{i-1}), 1) 15 | % equals 16 | % xR.U{i}. 17 | % 18 | % See also LEFT_ORTH_WITH_GAUGE 19 | 20 | % TTeMPS Toolbox. 21 | % Michael Steinlechner, 2013-2016 22 | % Questions and contact: michael.steinlechner@epfl.ch 23 | % BSD 2-clause license, see LICENSE.txt 24 | 25 | xR = orthogonalize( x, 1 ); 26 | 27 | [xL, G] = left_orth_with_gauge( xR ); 28 | 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetPartialGradient.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetPartialGradient(problem) 2 | % Checks whether the partial gradient can be computed for a given problem. 3 | % 4 | % function candoit = canGetPartialGradient(problem) 5 | % 6 | % Returns true if the partial gradient of the cost function can be computed 7 | % given the problem description, false otherwise. 8 | % 9 | % See also: getPartialGradient canGetPartialEuclideanGradient 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, June 28, 2016. 13 | % Contributors: 14 | % Change log: 15 | 16 | candoit = (isfield(problem, 'partialgrad') && isfield(problem, 'ncostterms')) || ... 17 | canGetPartialEuclideanGradient(problem); 18 | 19 | if isfield(problem, 'partialgrad') && ~isfield(problem, 'ncostterms') 20 | warning('manopt:partialgrad', ... 21 | ['If problem.partialgrad is specified, indicate the number n\n' ... 22 | 'of terms in the cost function with problem.ncostterms = n.']); 23 | end 24 | 25 | end 26 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/cnormsqfro.m: -------------------------------------------------------------------------------- 1 | function Xnormfro = cnormsqfro(X) 2 | % Computes the squared Frobenius norm of X 3 | % 4 | % function Xnormfro = cnormsqfro(X) 5 | % 6 | % Returns the squared Frobenius norm of X. This function can be seen as 7 | % norm(...,'fro')^2 but is compatible with dlarrays and structs with fields 8 | % real and imag. Supports both real and complex numbers. 9 | % 10 | % See also: manoptADhelp 11 | 12 | % This file is part of Manopt: www.manopt.org. 13 | % Original author: Xiaowen Jiang, July. 31, 2021. 14 | % Contributors: Nicolas Boumal 15 | % Change log: 16 | 17 | if iscstruct(X) 18 | Xnormfro = cinnerprodgeneral(X, X); 19 | 20 | elseif isnumeric(X) 21 | if isreal(X) 22 | Xnormfro = X(:)'*X(:); 23 | else 24 | Xnormfro = sum(real(conj(X(:)).*X(:))); 25 | end 26 | 27 | else 28 | ME = MException('cnormsqfro:inputError', ... 29 | 'Input does not have the expected format.'); 30 | throw(ME); 31 | end 32 | 33 | end 34 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/orthogonalize.m: -------------------------------------------------------------------------------- 1 | function x = orthogonalize( x, pos ) 2 | %ORTHOGONALIZE Orthogonalize tensor. 3 | % X = ORTHOGONALIZE( X, POS ) orthogonalizes all cores of the TTeMPS tensor X 4 | % except the core at position POS. Cores 1...POS-1 are left-, cores POS+1...end 5 | % are right-orthogonalized. Therefore, 6 | % 7 | % X = ORTHOGONALIZE( X, 1 ) right-orthogonalizes the full tensor, 8 | % 9 | % X = ORTHOGONALIZE( X, X.order ) left-orthogonalizes the full tensor. 10 | % 11 | % See also ORTH_AT. 12 | 13 | % TTeMPS Toolbox. 14 | % Michael Steinlechner, 2013-2014 15 | % Questions and contact: michael.steinlechner@epfl.ch 16 | % BSD 2-clause license, see LICENSE.txt 17 | 18 | % left orthogonalization till pos (from left) 19 | for i = 1:pos-1 20 | x = orth_at( x, i, 'left' ); 21 | end 22 | 23 | % right orthogonalization till pos (from right) 24 | for i = x.order:-1:pos+1 25 | x = orth_at( x, i, 'right' ); 26 | end 27 | end 28 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/matrixlincomb.m: -------------------------------------------------------------------------------- 1 | function v = matrixlincomb(x, a1, d1, a2, d2) %#ok 2 | % Linear combination function for tangent vectors represented as matrices. 3 | % 4 | % function v = lincomb(x, a1, d1) 5 | % function v = lincomb(x, a1, d1, a2, d2) 6 | % 7 | % Given a point x, two tangent vectors d1 and d2 at x, and two real 8 | % coefficients a1 and a2, returns a tangent vector at x representing 9 | % a1*d1 + a2*d2, if d1 and d2 are represented as matrices (or more 10 | % generally as arrays in Matlab). 11 | % 12 | % If a2 and d2 are omitted, the returned tangent vector is a1*d1. 13 | % 14 | % The input x is actually unused. 15 | % 16 | % This function is a helper to define manifolds in Manopt. 17 | 18 | % This file is part of Manopt: www.manopt.org. 19 | % Original author: Nicolas Boumal, July 2, 2015. 20 | % Contributors: 21 | % Change log: 22 | 23 | if nargin == 3 24 | v = a1*d1; 25 | elseif nargin == 5 26 | v = a1*d1 + a2*d2; 27 | else 28 | error('matrixlincomb takes either 3 or 5 inputs.'); 29 | end 30 | 31 | end 32 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/cnormfro.m: -------------------------------------------------------------------------------- 1 | function Xnormfro = cnormfro(X) 2 | % Computes the Frobenius norm of X 3 | % 4 | % function Xnormfro = cnormfro(X) 5 | % 6 | % Returns the Frobenius norm of X. This function can be seen as 7 | % norm(...,'fro') but is compatible with dlarrays and structs with fields 8 | % real and imag. Supports both real and complex numbers. 9 | % 10 | % See also: manoptADhelp 11 | 12 | % This file is part of Manopt: www.manopt.org. 13 | % Original author: Xiaowen Jiang, July. 31, 2021. 14 | % Contributors: Nicolas Boumal 15 | % Change log: 16 | 17 | if iscstruct(X) 18 | Xnormfro = sqrt(cinnerprodgeneral(X,X)); 19 | 20 | elseif isnumeric(X) 21 | if isreal(X) 22 | Xnormfro = sqrt(X(:)'*X(:)); 23 | else 24 | Xnormfro = sqrt(sum(real(conj(X(:)).*X(:)))); 25 | end 26 | 27 | else 28 | ME = MException('cnormfro:inputError', ... 29 | 'Input does not have the expected format.'); 30 | throw(ME); 31 | 32 | end 33 | 34 | end 35 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/completion/makeOmegaSet.m: -------------------------------------------------------------------------------- 1 | % TTeMPS Toolbox. 2 | % Michael Steinlechner, 2013-2016 3 | % Questions and contact: michael.steinlechner@epfl.ch 4 | % BSD 2-clause license, see LICENSE.txt 5 | function subs = makeOmegaSet( n, sizeOmega ) 6 | 7 | if sizeOmega > prod(n) 8 | error('makeOmegaSet:sizeOmegaTooHigh', 'Requested size of Omega is bigger than the tensor itself!') 9 | end 10 | 11 | idx = randi( prod(n), sizeOmega, 1 ); 12 | Omega = unique(idx); 13 | 14 | while length(Omega) < sizeOmega 15 | idx = [ Omega; randi( prod(n) , sizeOmega-length(Omega), 1 )]; 16 | Omega = unique(idx); 17 | end 18 | 19 | Omega = sort( Omega(1:sizeOmega) ); 20 | 21 | % get number of dimensions 22 | d = length(n); 23 | % prepare dynamically sized varargout for ind2sub 24 | % (careful! needs to be done because behaviour of ind2sub depends 25 | % on the number of output arguments) 26 | c = cell(1,d); 27 | [c{:}] = ind2sub( n, Omega ); 28 | 29 | subs = [c{:}]; 30 | end 31 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/csum.m: -------------------------------------------------------------------------------- 1 | function xsum = csum(x,n) 2 | % Sum of elements of x. 3 | % 4 | % function xsum = csum(x,n) 5 | % 6 | % This function can be seen as sum(x,n) but is compatible with 7 | % structs with fields real and imag. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July. 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if isstruct(x) && isfield(x,'real') 17 | if nargin==1 18 | xsum.real = sum(x.real); 19 | xsum.imag = sum(x.imag); 20 | elseif nargin==2 21 | xsum.real = sum(x.real,n); 22 | xsum.imag = sum(x.imag,n); 23 | end 24 | 25 | elseif isnumeric(x) 26 | if nargin==1 27 | xsum = sum(x); 28 | elseif nargin==2 29 | xsum = sum(x,n); 30 | end 31 | else 32 | ME = MException('csum:inputError', ... 33 | 'Input does not have the expected format.'); 34 | throw(ME); 35 | end 36 | 37 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/matricize.m: -------------------------------------------------------------------------------- 1 | function res = matricize( U, mode ) 2 | %MATRICIZE Matricize 3D Matlab array. 3 | % A = MATRICIZE(U, MODE) matricizes the 3D Matlab array U along the 4 | % specified mode MODE. Higher dimensions than 3 are not supported. 5 | % 6 | % See also TENSORIZE, TENSORPROD_TTEMPS, UNFOLD. 7 | 8 | % TTeMPS Toolbox. 9 | % Michael Steinlechner, 2013-2016 10 | % Questions and contact: michael.steinlechner@epfl.ch 11 | % BSD 2-clause license, see LICENSE.txt 12 | 13 | d = size(U); 14 | % pad with 1 for the last dim (annoying) 15 | if length(d) == 2 16 | d = [d, 1]; 17 | end 18 | 19 | switch mode 20 | case 1 21 | res = reshape( U, [d(1), d(2)*d(3)] ); 22 | case 2 23 | res = reshape( permute( U, [2, 1, 3]), [d(2), d(1)*d(3)] ); 24 | case 3 25 | res = transpose( reshape( U, [d(1)*d(2), d(3)] ) ); 26 | otherwise 27 | error('Invalid mode input in function matricize') 28 | end 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/lobpcg/README.md: -------------------------------------------------------------------------------- 1 | lobpcg 2 | ------- 3 | [![View Locally Optimal Block Preconditioned Conjugate Gradient on File Exchange](https://www.mathworks.com/matlabcentral/images/matlab-file-exchange.svg)](https://www.mathworks.com/matlabcentral/fileexchange/48-locally-optimal-block-preconditioned-conjugate-gradient) 4 | 5 | This main function LOBPCG is a version of the preconditioned conjugate gradient method (Algorithm 5.1) described in A. V. Knyazev, Toward the Optimal Preconditioned Eigensolver: Locally Optimal Block Preconditioned Conjugate Gradient Method, SIAM Journal on Scientific Computing 23 (2001), no. 2, pp. 517-541. http://dx.doi.org/10.1137/S1064827500366124 6 | A C-version of this code is a part of the https://github.com/lobpcg/blopex 7 | package and is available, e.g., in SLEPc and HYPRE. A scipy version is https://docs.scipy.org/doc/scipy/reference/generated/scipy.sparse.linalg.lobpcg.html 8 | Tested in MATLAB 6.5-7.13-R2019a and available Octave 3.2.3-3.4.2. 9 | Cite As 10 | Andrew Knyazev (2019). Locally Optimal Block Preconditioned Conjugate Gradient (https://www.github.com/lobpcg/blopex), GitHub. Retrieved December 4, 2019. -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/cdottimes.m: -------------------------------------------------------------------------------- 1 | function cdottimesAB = cdottimes(A, B) 2 | % Computes the element-wise multiplication between A and B 3 | % 4 | % function cdottimesAB = cdottimes(A, B) 5 | % 6 | % Returns the element-wise multiplication of A and B. The inputs A and B 7 | % can be either numeric arrays or structs with fields real and imag. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if isnumeric(A) && isnumeric(B) 17 | cdottimesAB = A .* B; 18 | 19 | elseif iscstruct(A) || iscstruct(B) 20 | A = tocstruct(A); 21 | B = tocstruct(B); 22 | cdottimesAB.real = A.real .* B.real - A.imag .* B.imag; 23 | cdottimesAB.imag = A.real .* B.imag + A.imag .* B.real; 24 | 25 | else 26 | ME = MException('cprod:inputError', ... 27 | 'Input does not have the expected format.'); 28 | throw(ME); 29 | 30 | end 31 | 32 | end 33 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/privateessential/essential_distMinAnglePair_test.m: -------------------------------------------------------------------------------- 1 | function essential_distMinAnglePair_test 2 | resetRands(3) 3 | flagDegenerateCase=true; 4 | k=2; 5 | 6 | e3=[0;0;1]; 7 | Q1=rot_randn([],[],2); 8 | if flagDegenerateCase 9 | Q1b=[Q1(:,:,1);Q1(:,:,2)]; 10 | Q2b=essential_randomVerticalMotion(Q1b); 11 | Q2=cat(3,Q2b(1:3,:),Q2b(4:6,:)); 12 | else 13 | Q2=rot_randn([],[],2); 14 | end 15 | Rzt=@(t) rot(t*e3); 16 | 17 | Q21tk=@(t,k) Rzt(t)*essential_flipAmbiguity_R1(Q2(:,:,1),k); 18 | Q22tk=@(t,k) Rzt(t)*essential_flipAmbiguity_R2(Q2(:,:,2),k); 19 | 20 | figure(1) 21 | [tMin,fMin,tBreak1,tBreak2,Q2Flip]=essential_distMinAnglePair([Q1(:,:,1);Q1(:,:,2)],[Q2(:,:,1);Q2(:,:,2)],k); 22 | tMin=modAngle(tMin); 23 | ft=@(t) (rot_dist(Q1(:,:,1),Q21tk(t,k))^2+rot_dist(Q1(:,:,2),Q22tk(t,k))^2); 24 | dft=@(t) 2*e3'*(Q1(:,:,1)*logrot(Q1(:,:,1)'*Q21tk(t,k))+Q1(:,:,2)*logrot(Q1(:,:,2)'*Q22tk(t,k))); 25 | check_der(ft,dft,'angle') 26 | hold on 27 | plot(tBreak1,ft(tBreak1),'r+') 28 | plot(tBreak2,ft(tBreak2),'g+') 29 | 30 | plot(tMin,fMin,'kx','MarkerSize',20) 31 | 32 | hold off 33 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/getsize.m: -------------------------------------------------------------------------------- 1 | function bytes = getsize(variable) 2 | % Estimates the amount of memory a given variable occupies, in bytes. 3 | % 4 | % Source: 5 | % https://ch.mathworks.com/matlabcentral/answers/14837-how-to-get-size-of-an-object 6 | % Posted by Dmitry Borovoy on Matlab Answers on Aug. 31, 2011. 7 | % Extended to structs by Mario Reutter on Matlab Answers on Dec. 13, 2017. 8 | % Slightly adapted by Victor Liao, Aug. 2022. 9 | 10 | % This file is part of Manopt: www.manopt.org. 11 | % Original authors: Dmitry Borovoy and Mario Reutter 12 | % Contributors: Victor Liao 13 | % Change log: 14 | 15 | props = properties(variable); 16 | if size(props, 1) < 1 17 | bytes = whos(varname(variable)); 18 | bytes = bytes.bytes; 19 | else % code of Dmitry 20 | bytes = 0; 21 | for ii = 1:length(props) 22 | currentProperty = getfield(variable, char(props(ii))); 23 | s = whos(varname(currentProperty)); 24 | bytes = bytes + s.bytes; 25 | end 26 | end 27 | end 28 | 29 | function name = varname(~) 30 | name = inputname(1); 31 | end 32 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/examples/example.m: -------------------------------------------------------------------------------- 1 | % TTeMPS Toolbox. 2 | % Michael Steinlechner, 2013-2016 3 | % Questions and contact: michael.steinlechner@epfl.ch 4 | % BSD 2-clause license, see LICENSE.txt 5 | 6 | rng(11); 7 | 8 | disp('Number of dimensions:') 9 | d = 5 10 | 11 | disp('Define tensor size:') 12 | n = [7 8 9 10 11] 13 | 14 | disp('Define rank vector (note that in the TT/MPS format, both the first and last rank are 1):') 15 | r = [1 4 5 6 7 1] 16 | 17 | disp('Create two random TT/MPS tensors:') 18 | X = TTeMPS_rand( r, n ) 19 | Y = TTeMPS_rand( r, n ) 20 | 21 | disp('Calculate inner product between them:') 22 | ip = innerprod( X, Y ) 23 | 24 | disp('Left-orthogonalize X:') 25 | X = orthogonalize( X, 1 ) 26 | 27 | disp('Right-orthogonalize Y:') 28 | Y = orthogonalize( Y, Y.order ) 29 | 30 | disp('Add X and Y') 31 | Z = X + Y 32 | 33 | disp('Truncate Z back to rank r:') 34 | Z_trunc = truncate(Z, r) 35 | 36 | disp('Note that we also have the round() operations,') 37 | disp('where you specify a desired accuracy instead of prescribed rank') 38 | Z_round = round(Z, 1e-2) 39 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/disp.m: -------------------------------------------------------------------------------- 1 | function disp( x, name ) 2 | %DISP Display TT/MPS tensor. 3 | % disp(X) displays the TT/MPS tensor X as a tensor network. 4 | 5 | % TTeMPS Toolbox. 6 | % Michael Steinlechner, 2013-2016 7 | % Questions and contact: michael.steinlechner@epfl.ch 8 | % BSD 2-clause license, see LICENSE.txt 9 | 10 | 11 | if (nargin < 2 || ~ischar(name)) 12 | name = inputname(1); 13 | end 14 | 15 | 16 | disp([name, ' is a TT/MPS tensor of order ', num2str(x.order), ... 17 | ' with size (', num2str(x.size), ... 18 | ') and ranks (', num2str(x.rank), ')']); 19 | disp(''); 20 | 21 | row1 = ''; 22 | row2 = ''; 23 | row3 = ''; 24 | 25 | for i=1:x.order 26 | row1 = [row1, sprintf('%3i--(U%2i)--', x.rank(i), i)]; 27 | row2 = [row2, ' | ']; 28 | row3 = [row3, sprintf(' %3i ', x.size(i))]; 29 | end 30 | row1 = [row1, sprintf( '%3i', x.rank(end) )]; 31 | disp(row1) 32 | disp(row2) 33 | disp(row3) 34 | 35 | end 36 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/left_orth_with_gauge.m: -------------------------------------------------------------------------------- 1 | function [xL, G] = left_orth_with_gauge( xR ) 2 | % LEFT_ORTH_WITH_GAUGE Left orthogonalization with storage of gauge matrices 3 | % 4 | % Given a right orthogonal X, compute a left orthogonalization and keep 5 | % the gauge matrices that relates them. 6 | % 7 | % The i-th core of xR 8 | % unfold(xR.U{i},'left') 9 | % is equal to the transformed i-th core of xL 10 | % kron(eye(n(i)),inv(G{i-1}))*unfold(xL.U{i},'left')*G{i} 11 | % (where fore i=1 and i=d, G{i} = 1). 12 | % 13 | % Or, equivalently 14 | % tensorprod_ttemps( tensorprod_ttemps( xL.U{i}, G{i}', 3), inv(G{i-1}), 1) 15 | % equals 16 | % xR.U{i}. 17 | % 18 | % See also GAUGE_MATRICES 19 | 20 | % TTeMPS Toolbox. 21 | % Michael Steinlechner, 2013-2016 22 | % Questions and contact: michael.steinlechner@epfl.ch 23 | % BSD 2-clause license, see LICENSE.txt 24 | 25 | xL = xR; 26 | G = cell(xR.order-1, 1); 27 | % left orthogonalization till pos (from left) 28 | for i = 1:xR.order-1 29 | [xL, G{i}] = orth_at( xL, i, 'left' ); 30 | end 31 | 32 | end 33 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetHessian.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetHessian(problem) 2 | % Checks whether the Hessian can be computed for a problem structure. 3 | % 4 | % function candoit = canGetHessian(problem) 5 | % 6 | % Returns true if the Hessian of the cost function can be computed given 7 | % the problem description, false otherwise. 8 | % 9 | % See also: canGetCost canGetDirectionalDerivative canGetGradient 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, Dec. 30, 2012. 13 | % Contributors: 14 | % Change log: 15 | 16 | candoit = isfield(problem, 'hess') || ... 17 | (isfield(problem, 'ehess') && canGetEuclideanGradient(problem)); 18 | 19 | % Display an extra warning message to the user in anticipation of 20 | % common mistakes. 21 | if ~candoit && ... 22 | (isfield(problem, 'ehess') && ~canGetEuclideanGradient(problem)) 23 | warning('manopt:canGetHessian', ... 24 | ['If the Hessian is supplied as a Euclidean Hessian (ehess),\n' ... 25 | 'then the Euclidean gradient must also be supplied (egrad).']); 26 | end 27 | 28 | end 29 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/mergeOptions.m: -------------------------------------------------------------------------------- 1 | function opts = mergeOptions(opts_sub, opts_master) 2 | % Merges two options structures with one having precedence over the other. 3 | % 4 | % function opts = mergeOptions(opts1, opts2) 5 | % 6 | % input: opts1 and opts2 are two structures. 7 | % output: opts is a structure containing all fields of opts1 and opts2. 8 | % Whenever a field is present in both opts1 and opts2, it is the value in 9 | % opts2 that is kept. 10 | % 11 | % The typical usage is to have opts1 contain default options and opts2 12 | % contain user-specified options that overwrite the defaults. 13 | % 14 | % See also: getGlobalDefaults 15 | 16 | % This file is part of Manopt: www.manopt.org. 17 | % Original author: Nicolas Boumal, Dec. 30, 2012. 18 | % Contributors: 19 | % Change log: 20 | 21 | 22 | if isempty(opts_sub) 23 | opts_sub = struct(); 24 | end 25 | if isempty(opts_master) 26 | opts_master = struct(); 27 | end 28 | 29 | opts = opts_sub; 30 | fields = fieldnames(opts_master); 31 | for i = 1 : length(fields) 32 | opts.(fields{i}) = opts_master.(fields{i}); 33 | end 34 | 35 | end 36 | -------------------------------------------------------------------------------- /MATLAB/utils/math_utils/vector_to_symmetric_matrix.m: -------------------------------------------------------------------------------- 1 | function sym_mat = vector_to_symmetric_matrix(vec, sparse_flag) 2 | if nargin < 2 3 | sparse_flag = false; 4 | end 5 | 6 | % This function converts a vector to a symmetric matrix The vector is 7 | % assumed to be one of the triangular parts of the matrix with the diagonal 8 | % elements divided by sqrt(2) s.t. the inner product of two vectors is 1/2 9 | % the inner product of the two matrices 10 | 11 | vec_len = length(vec); 12 | 13 | % vec_len = d*(d+1)/2 = d^2/2 + d/2 14 | % d^2 + d - 2*vec_len = 0 15 | d = (-1 + sqrt(1 + 8*vec_len))/2; 16 | assert (d * (d+1)/2 == vec_len) 17 | 18 | if sparse_flag 19 | sym_mat = sparse(d,d); 20 | else 21 | sym_mat = zeros(d,d); 22 | end 23 | 24 | idx = 1; 25 | [row_idxs, col_idxs] = find(triu(true(d))); 26 | assert (length(row_idxs) == vec_len) 27 | 28 | % scaling = 1 if offdiagonal, sqrt(2) if diagonal 29 | vec_scaling = (row_idxs == col_idxs) / sqrt(2) + (row_idxs ~= col_idxs); 30 | sym_mat(sub2ind(size(sym_mat), row_idxs, col_idxs)) = vec .* vec_scaling; 31 | 32 | sym_mat = sym_mat + sym_mat'; 33 | end -------------------------------------------------------------------------------- /MATLAB/utils/preconditioners/precon_function_factory.m: -------------------------------------------------------------------------------- 1 | function precon_function = precon_function_factory(problem, opts) 2 | if problem.use_marginalized 3 | A = problem.Qmain; 4 | else 5 | A = problem.Q; 6 | end 7 | 8 | if strcmp(opts.type, 'block_jacobi') 9 | precon_function = get_block_jacobi_preconditioner(A, opts.block_size); 10 | elseif strcmp(opts.type, 'ichol') 11 | precon_function = get_ichol_preconditioner(A); 12 | elseif strcmp(opts.type, 'jacobi') 13 | precon_function = get_jacobi_preconditioner(A); 14 | elseif strcmp(opts.type, 'regularized_cholesky') 15 | precon_function = get_regularized_cholesky_preconditioner(A, opts.condition_number_ub); 16 | elseif strcmp(opts.type, 'block_cholesky') 17 | precon_function = get_block_cholesky_preconditioner(problem); 18 | elseif strcmp(opts.type, 'none') 19 | precon_function = @(x) x; 20 | else 21 | supported_types = {'block_jacobi', 'ichol', 'jacobi', 'regularized_cholesky', 'none'}; 22 | error('Unknown preconditioner type: %s \nShould be one of: %s',... 23 | opts.type, strjoin(supported_types, ', ')); 24 | end 25 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/mtimes.m: -------------------------------------------------------------------------------- 1 | function x = mtimes( a, x ) 2 | %MINUS Multiplication of TT/MPS block tensor by scalar or vector 3 | % X = MTIMES(A, X) multiplies the TT/MPS tensor X by A. 4 | % If A is a scalar, all blocks are multiplied by the x.order-th root of A. 5 | % if A is a vector of size X.p, all X.p slices of the supercore X.U{x.mu} are multiplied 6 | % by the corresponding entry in A. 7 | % 8 | % See also PLUS, MINUS. 9 | 10 | % TTeMPS Toolbox. 11 | % Michael Steinlechner, 2013-2016 12 | % Questions and contact: michael.steinlechner@epfl.ch 13 | % BSD 2-clause license, see LICENSE.txt 14 | 15 | %x.U{1} = a*x.U{1}; 16 | 17 | if length(a) == 1 18 | c = a^(1/x.order); 19 | for i = 1:x.order 20 | x.U{i} = c*x.U{i}; 21 | end 22 | elseif length(a) == x.p 23 | for i = 1:x.p 24 | x.U{x.mu}(:,:,:,i) = a(i) * x.U{x.mu}(:,:,:,i); 25 | end 26 | else 27 | error('Dimension mismatch! Can only multiply block tensor X by scalar (whole tensor) or by a vector of length X.p') 28 | end 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/rotations/randunitary.m: -------------------------------------------------------------------------------- 1 | function U = randunitary(n, N) 2 | % Generates uniformly random unitary matrices. 3 | % 4 | % function U = randunitary(n, N) 5 | % 6 | % U is a n-by-n-by-N array such that each slice U(:, :, i) is a random 7 | % unitary matrix of size n (i.e., a matrix in the unitary group U(n)), 8 | % sampled from the Haar measure (uniform distribution). 9 | % 10 | % By default, N = 1. 11 | % 12 | % Complexity: N times O(n^3). 13 | % For details on the algorithm, see Mezzadri 2007, 14 | % "How to generate random matrices from the classical compact groups." 15 | % 16 | % See also: randrot qr_unique 17 | 18 | % This file is part of Manopt: www.manopt.org. 19 | % Original author: Nicolas Boumal, June 18, 2019. 20 | % Contributors: 21 | % Change log: 22 | 23 | if nargin < 2 24 | N = 1; 25 | end 26 | 27 | if n == 1 28 | U = sign(randn(1, 1, N) + 1i*randn(1, 1, N)); 29 | return; 30 | end 31 | 32 | % Generated as such, the slides of U are uniformly distributed over 33 | % U(n), the set of unitary matrices: see Mezzadri 2007, p597. 34 | U = qr_unique(randn(n, n, N) + 1i*randn(n, n, N)); 35 | 36 | end 37 | -------------------------------------------------------------------------------- /MATLAB/utils/io_utils/pose_graph/construct_pose_incidence_matrix.m: -------------------------------------------------------------------------------- 1 | function A = construct_pose_incidence_matrix(measurements, var_idx_mapping) 2 | %function A = construct_pose_incidence_matrix(measurements) 3 | % 4 | % This function computes and returns the oriented incidence matrix of the 5 | % underlying directed graph of measurements: 6 | % 7 | % A_{ie} = -1, if edge e *leaves* node i, 8 | % +1, if edge e *enters* node i, 9 | % 0, otherwise. 10 | % 11 | % (see eq. (7) in the paper). 12 | 13 | % Copyright (C) 2016 by David M. Rosen 14 | 15 | N = var_idx_mapping.pose_var_name_to_cnt.length; % N = number of nodes in the pose graph 16 | M = size(measurements.edges, 1); % Number of edges in the pose graph 17 | 18 | out_nodes = cell2mat(values(var_idx_mapping.pose_var_name_to_cnt, measurements.edges(:, 1))); 19 | in_nodes = cell2mat(values(var_idx_mapping.pose_var_name_to_cnt, measurements.edges(:, 2))); 20 | 21 | assert(all(out_nodes >= 1) && all(out_nodes <= N)); 22 | assert(all(in_nodes >= 1) && all(in_nodes <= N)); 23 | 24 | node_indices = [out_nodes, in_nodes]; 25 | edge_indices = [ [1:M], [1:M] ]; 26 | vals = [-ones(1, M), ones(1, M)]; 27 | 28 | A = sparse(node_indices, edge_indices, vals, N, M)'; 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/tensorize.m: -------------------------------------------------------------------------------- 1 | function res = tensorize( U, mode, d ) 2 | %TENSORIZE Tensorize matrix (inverse matricization). 3 | % X = TENSORIZE(U, MODE, D) (re-)tensorizes the matrix U along the 4 | % specified mode MODE into a tensor X of size D(1) x D(2) x D(3). Higher 5 | % dimensions than 3 are not supported. Tensorize is inverse matricization, 6 | % that is, X == tensorize( matricize(X, i), i, size(X)) for all modes i. 7 | % 8 | % See also MATRICIZE, TENSORPROD_TTEMPS, UNFOLD. 9 | 10 | % TTeMPS Toolbox. 11 | % Michael Steinlechner, 2013-2016 12 | % Questions and contact: michael.steinlechner@epfl.ch 13 | % BSD 2-clause license, see LICENSE.txt 14 | 15 | % pad with 1 for the last dim (annoying) 16 | if length(d) == 2 17 | d = [d, 1]; 18 | end 19 | 20 | switch mode 21 | case 1 22 | res = reshape( U, d ); 23 | case 2 24 | res = ipermute( reshape(U, [d(2), d(1), d(3)]), [2, 1, 3] ); 25 | case 3 26 | res = reshape( transpose(U), d ); 27 | otherwise 28 | error('Invalid mode input in function matricize') 29 | end 30 | end 31 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/findA_anchors.m: -------------------------------------------------------------------------------- 1 | function A = findA_anchors(problem) 2 | % Find the indices of the anchors for the anchoredrotationsfactory 3 | % 4 | % function A = findA_anchors(problem) 5 | % 6 | % Returns the indices of the anchors for the rotation matrices manifold. 7 | % 8 | % See also: anchoredrotationsfactory 9 | 10 | % This file is part of Manopt: www.manopt.org. 11 | % Original author: Xiaowen Jiang, Aug. 31, 2021. 12 | % Contributors: Nicolas Boumal 13 | % Change log: 14 | 15 | % check availability 16 | assert(isfield(problem,'M'),'problem structure must contain the field M.'); 17 | problem_name = problem.M.name(); 18 | % The manifold must be rotation matrices with anchors 19 | assert(contains(problem_name,'Product rotations manifold') &&..., 20 | contains(problem_name,'anchors') &&..., 21 | ~startsWith(problem_name,'Product manifold'),['The manifold must '... 22 | 'be rotation matrices with anchors']) 23 | % find indices of the anchors 24 | indexA = strfind(problem_name,'indices ') + 8; 25 | A = str2double(problem_name(indexA)); 26 | for i = indexA+3:3:length(problem_name) 27 | A = [A,str2double(problem_name(i))]; 28 | end 29 | 30 | end -------------------------------------------------------------------------------- /MATLAB/manopt/checkinstall/basicexample.m: -------------------------------------------------------------------------------- 1 | function basicexample() 2 | 3 | % Verify that Manopt was indeed added to the Matlab path. 4 | if isempty(which('spherefactory')) 5 | error(['You should first add Manopt to the Matlab path.\n' ... 6 | 'Please run importmanopt.']); 7 | end 8 | 9 | % Generate the problem data. 10 | n = 1000; 11 | A = randn(n); 12 | A = .5*(A+A'); 13 | 14 | % Create the problem structure. 15 | manifold = spherefactory(n); 16 | problem.M = manifold; 17 | 18 | % Define the problem cost function and its gradient. 19 | problem.cost = @(x) -x'*(A*x); 20 | problem.egrad = @(x) -2*A*x; 21 | problem.ehess = @(x, xdot) -2*A*xdot; 22 | 23 | % Numerically check gradient and Hessian consistency. 24 | figure; 25 | checkgradient(problem); 26 | figure; 27 | checkhessian(problem); 28 | 29 | % Solve. 30 | [x, xcost, info] = trustregions(problem); %#ok 31 | 32 | % Display some statistics. 33 | figure; 34 | semilogy([info.iter], [info.gradnorm], '.-'); 35 | xlabel('Iteration #'); 36 | ylabel('Gradient norm'); 37 | title('Convergence of the trust-regions algorithm on the sphere'); 38 | 39 | end 40 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetSqrtPrecon.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetSqrtPrecon(problem) 2 | % Checks whether a square root of preconditioner was specified in problem. 3 | % 4 | % function candoit = canGetSqrtPrecon(problem) 5 | % 6 | % Returns true if the problem structure allows for applying the square root 7 | % of a preconditioner to tangent vectors at a given point. The square root 8 | % of the preconditioner at x must be a symmetric, positive definite 9 | % operator Q such that applying Q twice (Q o Q) amounts to applying the 10 | % preconditioner once. If both a preconditioner and a square root of 11 | % preconditioner are provided, it is the user's responsibility to ensure 12 | % their compatibility. 13 | % 14 | % Similarly to getPrecon, if the present function returns false, calls to 15 | % getSqrtPrecon will still work: they will act as the identity. Note that 16 | % this may be incompatible with the preconditioner if it is given. Thus, 17 | % always check by calling canGetSqrtPrecon first. 18 | % 19 | % See also: canGetPrecon getSqrtPrecon getPrecon 20 | 21 | % This file is part of Manopt: www.manopt.org. 22 | % Original author: Nicolas Boumal, April 3, 2015. 23 | % Contributors: 24 | % Change log: 25 | 26 | candoit = isfield(problem, 'sqrtprecon'); 27 | 28 | end 29 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/mergecore.m: -------------------------------------------------------------------------------- 1 | function res = mergecore( x, idxL, idxR ) 2 | %MERGECORE Merging of two cores of a TT/MPS tensor. 3 | % 4 | % RES = MERGECORE(X, IDXL, IDXR) merges the two cores IDXL and IDXR of 5 | % the TT/MPS tensor X. IDXL and IDXR must be two consecutive integers in 6 | % ascending order. RES is a TT/MPS tensor of dimension X.order-1. 7 | % 8 | % See also SPLITCORE 9 | 10 | % TTeMPS Toolbox. 11 | % Michael Steinlechner, 2013-2016 12 | % Questions and contact: michael.steinlechner@epfl.ch 13 | % BSD 2-clause license, see LICENSE.txt 14 | 15 | if ~isscalar(idxL) 16 | error('Left index IDXL must be a scalar.') 17 | end 18 | if ~isscalar(idxR) 19 | error('Right index IDXR must be a scalar.') 20 | end 21 | if diff([idxL,idxR]) ~= 1 22 | error('Choose two neighboring nodes in ascending order.') 23 | end 24 | 25 | n = x.size; 26 | r = x.rank; 27 | 28 | mergedcore = unfold(x.U{idxL}, 'left') * unfold(x.U{idxR}, 'right'); 29 | mergedcore = reshape( mergedcore, [r(idxL), n(idxL)*n(idxR), r(idxR+1)] ); 30 | 31 | U = x.U; 32 | res = TTeMPS( {U{1:idxL-1}, mergedcore, U{idxR+1:end} } ); 33 | end 34 | 35 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/cplus.m: -------------------------------------------------------------------------------- 1 | function plusAB = cplus(A,B) 2 | % Computes the sum of A and B 3 | % 4 | % function plusAB = cplus(A,B) 5 | % 6 | % Returns the sum of A and B. This function can be seen as A+B but is 7 | % compatible with dlarrays and structs with fields real and imag. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July. 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if isnumeric(A) && iscstruct(B) 17 | realA = real(A); 18 | imagA = imag(A); 19 | plusAB.real = realA + B.real; 20 | plusAB.imag = imagA + B.imag; 21 | 22 | elseif iscstruct(A) && isnumeric(B) 23 | realB = real(B); 24 | imagB = imag(B); 25 | plusAB.real = realB + A.real; 26 | plusAB.imag = imagB + A.imag; 27 | 28 | elseif isnumeric(A) && isnumeric(B) 29 | plusAB = A + B; 30 | 31 | elseif iscstruct(A) && iscstruct(B) 32 | plusAB.real = A.real + B.real; 33 | plusAB.imag = A.imag + B.imag; 34 | 35 | else 36 | ME = MException('cplus:inputError', ... 37 | 'Input does not have the expected format.'); 38 | throw(ME); 39 | end 40 | 41 | end 42 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Certifiably Correct Range-Aided SLAM (CORA) 2 | 3 | This is a (no longer supported) MATLAB implementation build on [manopt](https://www.manopt.org/), primarily as a demo piece for extension and comparison. 4 | 5 | We recommend you look at our [official C++ implementation](https://github.com/MarineRoboticsGroup/cora) for a performant and actively maintained library. 6 | 7 | ## Examples 8 | 9 | Here is CORA in action on the Plaza1 and Plaza2 experiments from the 10 | [CMU Navigating with Ranging Radios Dataset](https://infoscience.epfl.ch/record/283435). 11 | In these animations the solver is initialized randomly to 12 | emphasize that CORA's performance is agnostic to the provided initialization. 13 | 14 | | Plaza2 | Plaza2 | 15 | | ------------| -----------| 16 | | ![Plaza2](media/pz1_grey_solid_inf_loop.gif) | ![Plaza2](media/pz2_grey_solid_inf_loop.gif) | 17 | 18 | 19 | ## Running Experiments 20 | 21 | You can recreate all of the experiments from our paper using the data and scripts in [this repository](https://github.com/MarineRoboticsGroup/cora-experiments/tree/main). 22 | 23 | ## Feedback, Support, and Contributions 24 | 25 | Any feedback, issues, or contributions are welcome and encouraged! Although the development on this is mostly dead, we'll do the best we can to provide support for hopeful users. 26 | 27 | Enjoy! 28 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/cminus.m: -------------------------------------------------------------------------------- 1 | function miusAB = cminus(A,B) 2 | % Computes the difference of A and B 3 | % 4 | % function miusAB = cminus(A,B) 5 | % 6 | % Returns the difference of A and B. This function can be seen as A-B but 7 | % is compatible with dlarrays and structs with fields real and imag. 8 | % 9 | % See also: manoptADhelp 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Xiaowen Jiang, July. 31, 2021. 13 | % Contributors: Nicolas Boumal 14 | % Change log: 15 | 16 | if isnumeric(A) && iscstruct(B) 17 | realA = real(A); 18 | imagA = imag(A); 19 | miusAB.real = realA - B.real; 20 | miusAB.imag = imagA - B.imag; 21 | 22 | elseif iscstruct(A) && isnumeric(B) 23 | realB = real(B); 24 | imagB = imag(B); 25 | miusAB.real = realB - A.real; 26 | miusAB.imag = imagB - A.imag; 27 | 28 | elseif isnumeric(A) && isnumeric(B) 29 | miusAB = A - B; 30 | 31 | elseif iscstruct(A) && iscstruct(B) 32 | miusAB.real = A.real - B.real; 33 | miusAB.imag = A.imag - B.imag; 34 | 35 | else 36 | ME = MException('cminus:inputError', ... 37 | 'Input does not have the expected format.'); 38 | throw(ME); 39 | end 40 | 41 | end 42 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/tensorprod_ttemps.m: -------------------------------------------------------------------------------- 1 | function res = tensorprod_ttemps( U, A, mode, apply_inv ) 2 | %TENSORPROD_TTEMPS Tensor-times-Matrix product. 3 | % A = TENSORPROD_TTEMPS(U, A, MODE) performs the mode-MODE product between the 4 | % tensor U and matrix A. Higher dimensions than 3 are not supported. 5 | % 6 | % A = TENSORPROD_TTEMPS(U, A, MODE, TRUE) multiplies with A^{-1} instead of A. 7 | % 8 | % Renamed from tensorprod to tensorprod on April 20, 2022 to accomodate the new 9 | % Matlab built-in function tensorprod in R2022a. 10 | % 11 | % See also MATRICIZE, TENSORIZE, UNFOLD. 12 | 13 | % TTeMPS Toolbox. 14 | % Michael Steinlechner, 2013-2016 15 | % Questions and contact: michael.steinlechner@epfl.ch 16 | % BSD 2-clause license, see LICENSE.txt 17 | 18 | if nargin==3 19 | apply_inv = false; 20 | end 21 | 22 | d = size(U); 23 | % pad with 1 for the last dim (annoying) 24 | if length(d) == 2 25 | d = [d, 1]; 26 | end 27 | d(mode) = size(A,1); 28 | 29 | if apply_inv 30 | res = A \ matricize( U, mode ); 31 | res = tensorize( res, mode, d ); 32 | else 33 | res = A * matricize( U, mode ); 34 | res = tensorize( res, mode, d ); 35 | end 36 | 37 | end 38 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/privateessential/essential_distMinAnglePair_dfNewton.m: -------------------------------------------------------------------------------- 1 | %Support function for essential_distMinAnglePair implementing Newton's search 2 | function [tMin,fMin]=essential_distMinAnglePair_dfNewton(m1,p1,c1,m2,p2,c2,tMin,tLow,tHigh) 3 | tolDist=1e-8; 4 | for i=1:100 5 | % d=dfi(m1,p1,c1,tMin)+dfi(m2,p2,c2,tMin); 6 | % dd=ddfi(m1,p1,c1,tMin)+ddfi(m2,p2,c2,tMin); 7 | %The code below unrolls the following calls 8 | % f1=fi(m1,p1,c1,tMin); 9 | % f2=fi(m2,p2,c2,tMin); 10 | % d=dfi2(m1,p1,f1,tMin)+dfi2(m2,p2,f2,tMin); 11 | % dd=ddfi2(m1,p1,f1,tMin)+ddfi2(m2,p2,f2,tMin); 12 | mc1=m1*cos(tMin+p1); 13 | mc2=m2*cos(tMin+p2); 14 | f1=acos(clip((m1*sin(tMin+p1)+c1-1)/2)); 15 | f2=acos(clip((m2*sin(tMin+p2)+c2-1)/2)); 16 | sf1=2*sin(f1); 17 | sf2=2*sin(f2); 18 | d1=-f1*mc1/sf1; 19 | d2=-f2*mc2/sf2; 20 | d=d1+d2; 21 | eztuSq1=(mc1/sf1)^2; 22 | dd1=eztuSq1+f1/2*cot(f1/2)*(1-eztuSq1); 23 | eztuSq2=(mc2/sf2)^2; 24 | dd2=eztuSq2+f2/2*cot(f2/2)*(1-eztuSq2); 25 | dd=dd1+dd2; 26 | 27 | 28 | tOld=tMin; 29 | tMin=max(tLow+tolDist,min(tHigh-tolDist,tOld-d/dd)); 30 | if abs(tMin-tOld) 0 27 | % Get the indices of the block 28 | block_indices = num_default_blocks*block_size+1:block_size_remainder; 29 | 30 | % Get the block 31 | block = A(block_indices, block_indices); 32 | 33 | % Compute the inverse of the block 34 | block_jacobi(block_indices, block_indices) = inv(block); 35 | end 36 | 37 | % Return the preconditioner function 38 | precon_function = @(x) block_jacobi * x; 39 | 40 | end 41 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/grammatrix.m: -------------------------------------------------------------------------------- 1 | function G = grammatrix(M, x, vectors) 2 | % Computes the Gram matrix of tangent vectors in the Manopt framework. 3 | % 4 | % function G = grammatrix(M, x, vectors) 5 | % 6 | % M is a Manopt manifold structure obtained from a factory. 7 | % x is a point on the manifold M. 8 | % vectors is a cell containing n tangent vectors at x. 9 | % 10 | % G is an n-by-n symmetric positive semidefinite matrix such that G(i, j) 11 | % is the inner product between vectors{i} and vectors{j}, with respect to 12 | % the metric on the tangent space to M at x. 13 | % 14 | % See also: orthogonalize tangentorthobasis 15 | 16 | % This file is part of Manopt: www.manopt.org. 17 | % Original author: Nicolas Boumal, April 28, 2016. 18 | % Contributors: 19 | % Change log: 20 | 21 | 22 | n = numel(vectors); 23 | 24 | G = zeros(n); 25 | 26 | for i = 1 : n 27 | 28 | vi = vectors{i}; 29 | 30 | G(i, i) = M.inner(x, vi, vi); 31 | 32 | for j = (i+1) : n 33 | 34 | vj = vectors{j}; 35 | G(i, j) = M.inner(x, vi, vj); 36 | 37 | % Manopt is designed to work with real inner products, 38 | % but it does not hurt to allow for complex inner products 39 | % here by taking the conjugate. 40 | G(j, i) = G(i, j)'; 41 | 42 | end 43 | 44 | end 45 | 46 | end 47 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/lyapunov_symmetric_eig.m: -------------------------------------------------------------------------------- 1 | function X = lyapunov_symmetric_eig(V, lambda, C, tol) 2 | % Solves AX + XA = C when A = A', as a pseudo-inverse, given eig(A). 3 | % 4 | % function X = lyapunov_symmetric_eig(V, lambda, C) 5 | % function X = lyapunov_symmetric_eig(V, lambda, C, tol) 6 | % 7 | % Same as lyapunov_symmetric(A, C, [tol]), where A is symmetric, its 8 | % eigenvalue decomposition [V, lambda] = eig(A, 'vector') is provided as 9 | % input directly, and C is a single matrix of the same size as A. 10 | % 11 | % See also: lyapunov_symmetric sylvester lyap sylvester_nocheck 12 | 13 | % This file is part of Manopt: www.manopt.org. 14 | % Original author: Nicolas Boumal, Aug. 31, 2018. 15 | % Contributors: 16 | % Change log: 17 | 18 | % AX + XA = C is equivalent to DY + YD = M with 19 | % Y = V'XV, M = V'CV and D = diag(lambda). 20 | M = V'*C*V; 21 | 22 | % W(i, j) = lambda(i) + lambda(j) 23 | W = bsxfun(@plus, lambda, lambda'); 24 | 25 | % Normally, the solution Y is simply this: 26 | Y = M ./ W; 27 | 28 | % But this may involve divisions by (almost) 0 in certain places. 29 | % Thus, we go for a pseudo-inverse. 30 | absW = abs(W); 31 | if ~exist('tol', 'var') || isempty(tol) 32 | tol = numel(C)*eps(max(absW(:))); % similar to pinv tolerance 33 | end 34 | Y(absW <= tol) = 0; 35 | 36 | % Undo the change of variable 37 | X = V*Y*V'; 38 | 39 | end 40 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multiprodmultitransp_license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2009, Paolo de Leva 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/plus.m: -------------------------------------------------------------------------------- 1 | function z = plus( x, y ) 2 | %PLUS Addition of two TT/MPS tensors. 3 | % Z = PLUS(X,Y) adds two TT/MPS tensors. The rank of the resulting 4 | % tensor is 2*R. 5 | % 6 | % See also MINUS, UMINUS. 7 | 8 | % TTeMPS Toolbox. 9 | % Michael Steinlechner, 2013-2016 10 | % Questions and contact: michael.steinlechner@epfl.ch 11 | % BSD 2-clause license, see LICENSE.txt 12 | 13 | % add sanity check... 14 | rx = x.rank; 15 | ry = y.rank; 16 | nx = x.size; 17 | 18 | z = TTeMPS( cell(1, x.order) ); 19 | 20 | % first core: 21 | p = size(x.U{1},4); 22 | tmp = zeros( 1, nx(1), rx(2)+ry(2), p ); 23 | tmp( 1, :, 1:rx(2), : ) = x.U{1}; 24 | tmp( 1, :, rx(2)+1:end, : ) = y.U{1}; 25 | z.U{1} = tmp; 26 | 27 | % central cores: 28 | for i = 2:x.order-1 29 | % possibility of block format: 30 | p = size(x.U{i},4); 31 | tmp = zeros( rx(i)+ry(i), nx(i), rx(i+1)+ry(i+1), p); 32 | tmp( 1:rx(i), :, 1:rx(i+1), :) = x.U{i}; 33 | tmp( rx(i)+1:end, :, rx(i+1)+1:end, :) = y.U{i}; 34 | z.U{i} = tmp; 35 | end 36 | 37 | % last core: 38 | p = size(x.U{end},4); 39 | tmp = zeros( rx(end-1)+ry(end-1), nx(end), 1, p ); 40 | tmp( 1:rx(end-1), :, 1, : ) = x.U{end}; 41 | tmp( rx(end-1)+1:end, :, 1, : ) = y.U{end}; 42 | z.U{end} = tmp; 43 | end 44 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/splitcore.m: -------------------------------------------------------------------------------- 1 | function res = splitcore( x, idx, nL, nR, tol ) 2 | %SPLITCORE Merging of two cores of a TT/MPS tensor. 3 | % 4 | % RES = SPLITCORE(X,IDX,NL,NR) splits the core IDX of 5 | % the TT/MPS tensor X into two cores with outer dimensions NL and NR. 6 | % The new outer dimensions NL and NR have to fulfill NL*NR = X.size(idx) 7 | % 8 | % See also MERGECORE 9 | 10 | % TTeMPS Toolbox. 11 | % Michael Steinlechner, 2013-2016 12 | % Questions and contact: michael.steinlechner@epfl.ch 13 | % BSD 2-clause license, see LICENSE.txt 14 | 15 | if ~exist('tol', 'var') 16 | tol = 1e-8; 17 | end 18 | 19 | if ~isscalar(idx) 20 | error('Index IDX must be a scalar.') 21 | end 22 | 23 | n = x.size; 24 | r = x.rank; 25 | 26 | if nL*nR ~= n(idx) 27 | error('New sizes must be compatible with old tensor: NL*NR = X.size(idx)') 28 | end 29 | 30 | %x = orthogonalize(x, idx); 31 | [U,S,V] = svd( reshape(x.U{idx}, [r(idx)*nL, nR*r(idx+1)]), 'econ'); 32 | s = trunc_singular( diag(S), tol, true ); 33 | U = U(:,1:s); 34 | V = V(:,1:s); 35 | S = S(1:s,1:s); 36 | newcoreR = reshape( S*V', [s, nR, r(idx+1)] ); 37 | newcoreL = reshape( U, [r(idx), nL, s] ); 38 | 39 | C = x.U; 40 | res = TTeMPS( {C{1:idx-1}, newcoreL, newcoreR, C{idx+1:end} } ); 41 | end 42 | 43 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # matlab 2 | MATLAB/examples/* 3 | help 4 | data/* 5 | diary 6 | MATLAB/*.mat 7 | *.asv 8 | cora_main.m 9 | 10 | # OS generated files # 11 | ###################### 12 | .DS_Store 13 | .DS_Store? 14 | ._* 15 | .Spotlight-V100 16 | .Trashes 17 | ehthumbs.db 18 | [Tt]humbs.db 19 | 20 | # mypy 21 | *.sqlite3 22 | .mypy_cache 23 | 24 | # python installations 25 | *.egg-info 26 | 27 | # Jupyter Notebooks # 28 | ##################### 29 | .ipynb_checkpoints 30 | 31 | # Binaries # 32 | ############ 33 | *.pyc 34 | 35 | # Latex # 36 | ######### 37 | *.aux 38 | *.fdb_latexmk 39 | *.fls 40 | *.log 41 | *.out 42 | *.synctex.gz 43 | *.bbl 44 | *.blg 45 | *.bcf 46 | *.run.xml 47 | *.toc 48 | *.dvi 49 | *.lof 50 | *.lot 51 | main.pdf 52 | 53 | # VSCode # 54 | ############# 55 | .history/ 56 | .vscode/ 57 | .history/* 58 | .vscode/* 59 | .markdownlint.json 60 | *.code-workspace 61 | 62 | # spacemacs # 63 | ############# 64 | auto/ 65 | .#* 66 | 67 | # Python # 68 | ########## 69 | __pycache__/ 70 | test.py 71 | 72 | 73 | # PyTorch # 74 | ########### 75 | *.pt 76 | 77 | # Executables/C++ # 78 | ################### 79 | build/ 80 | 81 | 82 | # Tensorflow # 83 | ############## 84 | *.tfrecord 85 | *.pb 86 | 87 | # Temporary Files # 88 | ################### 89 | *.*~ 90 | *.swp 91 | 92 | # Blender # 93 | ########### 94 | *.blend 95 | 96 | # MATLAB # 97 | ########## 98 | *.m~ 99 | 100 | # ROS # 101 | ####### 102 | *.bag 103 | -------------------------------------------------------------------------------- /MATLAB/manopt/importmanopt.m: -------------------------------------------------------------------------------- 1 | function importmanopt() 2 | % Add Manopt to the path to make all manopt components available. 3 | 4 | % This file is part of Manopt: www.manopt.org. 5 | % Original author: Nicolas Boumal, Jan. 3, 2013. 6 | % Contributors: 7 | % Change log: 8 | % Aug. 7, 2013 (NB): Changed to work without the import command 9 | % (new structure of the toolbox). 10 | % Aug. 8, 2013 (NB): Changed to use addpath_recursive, home brewed. 11 | % Aug. 22, 2013 (NB): Using genpath instead of home cooked 12 | % addpath_recursive. 13 | % Sep. 5, 2022 (NB): Added some safe-guards against pre-defined vars. 14 | 15 | addpath(pwd()); 16 | 17 | % Recursively add Manopt directories to the Matlab path. 18 | cd('manopt'); 19 | addpath(genpath(pwd())); 20 | cd('..'); 21 | 22 | % Ask user if the path should be saved or not 23 | fprintf('Manopt was added to Matlab''s path.\n'); 24 | response = input('Save path for future Matlab sessions? [Y/N] ', 's'); 25 | if strcmpi(response, 'Y') 26 | failed = savepath(); 27 | if ~failed 28 | fprintf('Path saved: no need to call importmanopt next time.\n'); 29 | else 30 | fprintf(['Something went wrong.. Perhaps missing permission ' ... 31 | 'to write on pathdef.m?\nPath not saved: ' ... 32 | 'please re-call importmanopt next time.\n']); 33 | end 34 | else 35 | fprintf('Path not saved: please re-call importmanopt next time.\n'); 36 | end 37 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/install_mex.m: -------------------------------------------------------------------------------- 1 | % Install helper for mex functions 2 | 3 | % TTeMPS Toolbox. 4 | % Michael Steinlechner, 2013-2016 5 | % Questions and contact: michael.steinlechner@epfl.ch 6 | % BSD 2-clause license, see LICENSE.txt 7 | 8 | % MAC AND LINUX: 9 | if strcmp(filesep,'/') 10 | disp('Compiling for Mac or Linux system ...') 11 | mex -lmwlapack -lmwblas -largeArrayDims @TTeMPS/subsref_mex.c -outdir @TTeMPS 12 | mex -lmwlapack -lmwblas -largeArrayDims @TTeMPS_tangent/TTeMPS_tangent_omega.c -outdir @TTeMPS_tangent 13 | mex -lmwlapack -lmwblas -largeArrayDims @TTeMPS_tangent_orth/TTeMPS_tangent_orth_omega.c -outdir @TTeMPS_tangent_orth 14 | mex -lmwlapack -lmwblas -largeArrayDims algorithms/completion/als_solve_mex.c -outdir algorithms/completion/ 15 | 16 | % WINDOWS: 17 | elseif strcmp(filesep, '\') 18 | disp('Compiling for Windows system ...') 19 | mex -lmwlapack -lmwblas -largeArrayDims @TTeMPS\subsref_mex.c -outdir @TTeMPS 20 | mex -lmwlapack -lmwblas -largeArrayDims @TTeMPS_tangent\TTeMPS_tangent_omega.c -outdir @TTeMPS_tangent 21 | mex -lmwlapack -lmwblas -largeArrayDims @TTeMPS_tangent_orth\TTeMPS_tangent_orth_omega.c -outdir @TTeMPS_tangent_orth 22 | mex -lmwlapack -lmwblas -largeArrayDims algorithms\completion\als_solve_mex.c -outdir algorithms\completion 23 | mex -lmwlapack -lmwblas -largeArrayDims ..\weingarten_omega.c -outdir .. 24 | 25 | else 26 | disp('Unknown filesep. Compile manually. Aborting.') 27 | end 28 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/identify_linear_piece.m: -------------------------------------------------------------------------------- 1 | function [range, poly] = identify_linear_piece(x, y, window_length) 2 | % Identify a segment of the curve (x, y) that appears to be linear. 3 | % 4 | % function [range poly] = identify_linear_piece(x, y, window_length) 5 | % 6 | % This function attempts to identify a contiguous segment of the curve 7 | % defined by the vectors x and y that appears to be linear. A line is fit 8 | % through the data over all windows of length window_length and the best 9 | % fit is retained. The output specifies the range of indices such that 10 | % x(range) is the portion over which (x, y) is the most linear and the 11 | % output poly specifies a first order polynomial that best fits (x, y) over 12 | % that range, following the usual matlab convention for polynomials 13 | % (highest degree coefficients first). 14 | % 15 | % See also: checkdiff checkgradient checkhessian 16 | 17 | % This file is part of Manopt: www.manopt.org. 18 | % Original author: Nicolas Boumal, July 8, 2013. 19 | % Contributors: 20 | % Change log: 21 | 22 | residues = zeros(length(x)-window_length, 1); 23 | polys = zeros(2, length(residues)); 24 | for i = 1 : length(residues) 25 | range = i:(i+window_length); 26 | [poly, meta] = polyfit(x(range), y(range), 1); 27 | residues(i) = meta.normr; 28 | polys(:, i) = poly'; 29 | end 30 | [unused, best] = min(residues); %#ok 31 | range = best:(best+window_length); 32 | poly = polys(:, best)'; 33 | 34 | end 35 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/essential/essential_ehessE2ehess.m: -------------------------------------------------------------------------------- 1 | function ehess = essential_ehessE2ehess(X, egradE, ehessE, S) 2 | % Converts the Hessian in essential matrix E to the Hessian in X. 3 | % 4 | % function ehess = essential_ehessE2ehess(X, egradE, ehessE, S) 5 | % 6 | % egradE is the function handle for the gradient in E. 7 | % ehessE is the function handle for the Hessian in E. 8 | % S is the search direction in the space of X. 9 | % 10 | % The output is a matrix in the space of X. 11 | % 12 | % See also: essential_costE2cost essential_egradE2egrad 13 | 14 | 15 | % This file is part of Manopt: www.manopt.org. 16 | % Original author: Roberto Tron, Aug. 8, 2014 17 | % Contributors: Bamdev Mishra, May 22, 2015. 18 | 19 | e3hat = [0 -1 0; 1 0 0; 0 0 0]; 20 | 21 | RA = X(:,1:3,:); 22 | RB = X(:,4:6,:); 23 | E = multiprod(multiprod(multitransp(RA), e3hat), RB); % M.E(X); 24 | G = egradE(E); 25 | 26 | V = essential_sharp(multiprod(essential_flat(X), essential_flat(S))); 27 | VA = V(:,1:3,:); 28 | VB = V(:,4:6,:); 29 | 30 | dE = multiprod(multiprod(multitransp(RA), e3hat), VB)... 31 | + multiprod(multiprod(multitransp(VA), e3hat), RB); 32 | dG = ehessE(E, dE); 33 | 34 | %The following is the vectorized version of ehess = e3hat*[(VB*G'+RB*H') -(VA*G+RA*H)] 35 | ehess = multiprod(e3hat,cat(2,... 36 | multiprod(VB, multitransp(G)) + multiprod(RB, multitransp(dG)),... 37 | -multiprod(VA, G) - multiprod(RA, dG))); 38 | 39 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/linearsystem/solvePrecond.m: -------------------------------------------------------------------------------- 1 | % TTeMPS Toolbox. 2 | % Michael Steinlechner, 2013-2016 3 | % Questions and contact: michael.steinlechner@epfl.ch 4 | % BSD 2-clause license, see LICENSE.txt 5 | 6 | function xi = solvePrecond(L, P, rhs, xL, xR, opts ) 7 | 8 | if nargin==5 9 | opts.precond_tol = 1e-5; % 1e-8 has the same performace, just to be sure... 10 | opts.precond_maxit = 5; 11 | end 12 | 13 | P_rhs = precond_laplace( P, rhs ); 14 | 15 | 16 | 17 | function y = fun_A(x) 18 | % x is a vectorized tangent 19 | x_tangent = fill_with_vectorized( dummy, x ); 20 | x_ttemps = tangent_to_TTeMPS( x_tangent ); 21 | y_ttemps = apply(L, x_ttemps); 22 | y_tangent = TTeMPS_tangent_orth( xL, xR, y_ttemps ); 23 | y = vectorize_tangent( y_tangent ); 24 | end 25 | 26 | function y = fun_P(x) 27 | x_tangent = fill_with_vectorized( dummy, x ); 28 | eta = precond_laplace( P, x_tangent ); 29 | y = vectorize_tangent( eta ); 30 | end 31 | 32 | if opts.precond_maxit > 1 33 | dummy = rhs; 34 | 35 | tol = opts.precond_tol; 36 | maxit = opts.precond_maxit-1; % minus one !! 37 | 38 | [xi_vec,flag,relres,iter] = pcg(@fun_A, vectorize_tangent(rhs), tol, maxit, @fun_P, [], vectorize_tangent(P_rhs) ); 39 | iter 40 | xi = fill_with_vectorized( dummy, xi_vec ); 41 | else 42 | xi = P_rhs; 43 | end 44 | 45 | end 46 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op/plus.m: -------------------------------------------------------------------------------- 1 | function z = plus( x, y ) 2 | %PLUS Addition of two TT/MPS operators. 3 | % Z = PLUS(X,Y) adds to TT/MPS operators. The resulting TT/MPS operator 4 | % has rank equal to the sum of the individual ranks. 5 | % 6 | % TTeMPS Toolbox. 7 | % Michael Steinlechner, 2013-2016 8 | % Questions and contact: michael.steinlechner@epfl.ch 9 | % BSD 2-clause license, see LICENSE.txt 10 | 11 | 12 | % add sanity check... 13 | rx = x.rank; 14 | ry = y.rank; 15 | 16 | z = TTeMPS_op( cell(1, x.order) ); 17 | 18 | % first core: 19 | tmp = zeros( 1, x.size_col(1), x.size_row(1), rx(2)+ry(2) ); 20 | tmp( 1, :, :, 1:rx(2) ) = x.U{1}; 21 | tmp( 1, :, :, rx(2)+1:end ) = y.U{1}; 22 | z.U{1} = tmp; 23 | 24 | %z.U{1} = reshape( [unfold( x.U{1}, 'left'), unfold( y.U{1}, 'left')], [1, x.size(1), x.rank(2) + y.rank(2)]); 25 | 26 | % central cores: 27 | for i = 2:x.order-1 28 | tmp = zeros( rx(i)+ry(i), x.size_col(i), x.size_row(i), rx(i+1)+ry(i+1) ); 29 | tmp( 1:rx(i), :, :, 1:rx(i+1) ) = x.U{i}; 30 | tmp( rx(i)+1:end, :, :, rx(i+1)+1:end ) = y.U{i}; 31 | z.U{i} = tmp; 32 | end 33 | 34 | % last core: 35 | tmp = zeros( rx(end-1)+ry(end-1), x.size_col(end), x.size_row(end), 1 ); 36 | tmp( 1:rx(end-1), :, :, 1 ) = x.U{end}; 37 | tmp( rx(end-1)+1:end, :, :, 1 ) = y.U{end}; 38 | z.U{end} = tmp; 39 | 40 | z = update_properties( z ); 41 | end 42 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/lincomb.m: -------------------------------------------------------------------------------- 1 | function vec = lincomb(M, x, vecs, coeffs) 2 | % Computes a linear combination of tangent vectors in the Manopt framework. 3 | % 4 | % vec = lincomb(M, x, vecs, coeffs) 5 | % 6 | % M is a Manopt manifold structure obtained from a factory. 7 | % x is a point on the manifold M. 8 | % vecs is a cell containing n tangent vectors at x. 9 | % coeffs is a vector of length n 10 | % 11 | % vec is a tangent vector at x obtained as the linear combination 12 | % 13 | % vec = coeffs(1)*vecs{1} + ... + coeffs(n)*vecs{n} 14 | % 15 | % If vecs is an orthonormal basis, then tangent2vec is the inverse of 16 | % lincomb. 17 | % 18 | % See also: grammatrix orthogonalize tangentorthobasis tangent2vec 19 | 20 | % This file is part of Manopt: www.manopt.org. 21 | % Original author: Nicolas Boumal, April 28, 2016. 22 | % Contributors: 23 | % Change log: 24 | 25 | 26 | n = numel(vecs); 27 | assert(numel(coeffs) == n); 28 | 29 | switch n 30 | 31 | case 0 32 | 33 | vec = M.zerovec(x); 34 | 35 | case 1 36 | 37 | vec = M.lincomb(x, coeffs(1), vecs{1}); 38 | 39 | otherwise 40 | 41 | vec = M.lincomb(x, coeffs(1), vecs{1}, coeffs(2), vecs{2}); 42 | 43 | for k = 3 : n 44 | 45 | vec = M.lincomb(x, 1, vec, coeffs(k), vecs{k}); 46 | 47 | end 48 | 49 | end 50 | 51 | 52 | end 53 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/tangent2vec.m: -------------------------------------------------------------------------------- 1 | function vec = tangent2vec(M, x, basis, u) 2 | % Expands a tangent vector into an orthonormal basis in the Manopt framework 3 | % 4 | % vec = tangent2vec(M, x, basis, u) 5 | % 6 | % The inverse operation is lincomb (see below). 7 | % 8 | % M is a Manopt manifold structure obtained from a factory. 9 | % x is a point on the manifold M. 10 | % basis is a cell containing n orthonormal tangent vectors at x, forming an 11 | % orthonormal basis of the tangent space at x. 12 | % u is a tangent vector at x 13 | % 14 | % vec is a column vector of length n which contains the coefficients of the 15 | % expansion of u into the basis. Thus: 16 | % 17 | % vec(k) = _x <- vec = tangent2vec(M, x, basis, u) 18 | % 19 | % u = sum_{k=1}^n vec(k)*basis{k} <- u = lincomb(M, x, basis, vec) 20 | % 21 | % Note that tangent2vec is an isometry, that is, up to numerical round-off 22 | % errors, with u and v two tangent vectors at x: 23 | % 24 | % M.inner(x, u, v) == uu'*vv, 25 | % 26 | % where uu = tangent2vec(M, x, basis, u), vv = tangent2vec(M, x, basis, v). 27 | % 28 | % See also: lincomb tangentorthobasis orthogonalize grammatrix hessianmatrix 29 | 30 | % This file is part of Manopt: www.manopt.org. 31 | % Original author: Nicolas Boumal, Feb. 3, 2017. 32 | % Contributors: 33 | % Change log: 34 | 35 | 36 | n = numel(basis); 37 | 38 | vec = zeros(n, 1); 39 | 40 | for k = 1 : n 41 | 42 | vec(k) = M.inner(x, basis{k}, u); 43 | 44 | end 45 | 46 | end 47 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multiscale.m: -------------------------------------------------------------------------------- 1 | function A = multiscale(scale, A) 2 | % Multiplies the 2D slices in a 3D matrix by individual scalars. 3 | % 4 | % function A = multiscale(scale, A) 5 | % 6 | % Given a vector scale of length N and a 3-D array A of size 7 | % n-by-m-by-N, returns an array B of same size as A such that 8 | % B(:, :, k) = scale(k) * A(:, :, k); 9 | % 10 | % See also: multiprod multitransp multitrace cmultiscale 11 | 12 | % This file is part of Manopt: www.manopt.org. 13 | % Original author: Nicolas Boumal, Dec. 30, 2012. 14 | % Contributors: 15 | % Change log: 16 | % Aug. 29, 2021 (NB): 17 | % Corrected bug that occurred for complex 'scale' vector. 18 | % Sep. 5, 2021 (NB): 19 | % Using .* rather than bxsfun as a preferred way: this is faster. 20 | % Kept the bsxfun code in a try/catch in case this causes trouble 21 | % with older versions of Matlab (unsure whether it would). 22 | 23 | assert(ndims(A) <= 3, ... 24 | ['multiscale is only defined for arrays of 3 or fewer ' ... 25 | 'dimensions.']); 26 | 27 | [n, m, N] = size(A); 28 | 29 | assert(numel(scale) == N, ... 30 | ['scale must be a vector whose length equals the third ' ... 31 | 'dimension of A, that is, the number of 2D matrix slices ' ... 32 | 'in the 3D array A.']); 33 | 34 | try 35 | A = A .* reshape(scale, [1, 1, N]); 36 | catch 37 | scale = scale(:); 38 | A = reshape(bsxfun(@times, reshape(A, n*m, N), scale.'), n, m, N); 39 | end 40 | 41 | end 42 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/solvers/neldermead/centroid.m: -------------------------------------------------------------------------------- 1 | function y = centroid(M, x) 2 | % Attempts the computation of a centroid of a set of points on a manifold. 3 | % 4 | % function y = centroid(M, x) 5 | % 6 | % M is a structure representing a manifold. 7 | % x is a cell of points on that manifold. 8 | 9 | % This file is part of Manopt: www.manopt.org. 10 | % Original author: Nicolas Boumal, Dec. 30, 2012. 11 | % Contributors: 12 | % Change log: 13 | 14 | 15 | % For now, just apply a few steps of gradient descent for Karcher means 16 | 17 | n = numel(x); 18 | 19 | problem.M = M; 20 | 21 | problem.cost = @cost; 22 | function val = cost(y) 23 | val = 0; 24 | for i = 1 : n 25 | val = val + M.dist(y, x{i})^2; 26 | end 27 | val = val/2; 28 | end 29 | 30 | problem.grad = @grad; 31 | function g = grad(y) 32 | g = M.zerovec(y); 33 | for i = 1 : n 34 | g = M.lincomb(y, 1, g, -1, M.log(y, x{i})); 35 | end 36 | end 37 | 38 | % This line can be uncommented to check that the gradient is indeed 39 | % correct. This should always be the case if the dist and the log 40 | % functions in the manifold are correct. 41 | % checkgradient(problem); pause; 42 | 43 | query = warning('query', 'manopt:getHessian:approx'); 44 | warning('off', 'manopt:getHessian:approx'); 45 | options.verbosity = 0; 46 | options.maxiter = 15; 47 | y = trustregions(problem, x{randi(n)}, options); 48 | warning(query.state, 'manopt:getHessian:approx'); 49 | 50 | end 51 | -------------------------------------------------------------------------------- /MATLAB/utils/io_utils/load_ra_slam_problem.m: -------------------------------------------------------------------------------- 1 | function problem = load_ra_slam_problem(mats_filepath) 2 | 3 | fprintf('Loading problem from %s\n', mats_filepath) 4 | 5 | problem = struct(); 6 | 7 | % load the data from the file 8 | mats = load(mats_filepath); 9 | 10 | % copy over everything from mats to problem 11 | fields = fieldnames(mats); 12 | for i = 1:length(fields) 13 | 14 | % skip the file if it contains "idxs" - we have to handle those a bit 15 | % differently (see below) 16 | if ~isempty(strfind(fields{i}, 'idxs')) 17 | continue; 18 | end 19 | data = mats.(fields{i}); 20 | 21 | % if the field begins with "num_", then let's cast it to a double 22 | if strfind(fields{i}, 'num_') == 1 23 | data = double(data); 24 | end 25 | 26 | % if the field is "dim" let's cast it to a double 27 | if strcmp(fields{i}, 'dim') 28 | data = double(data); 29 | end 30 | 31 | problem.(fields{i}) = data; 32 | end 33 | 34 | 35 | % indices of variables 36 | problem.all_R_idxs = mats.rot_idxs+1; 37 | problem.all_t_idxs = mats.tran_idxs+1; 38 | problem.all_l_idxs = mats.beacon_idxs+1; 39 | problem.all_d_idxs = mats.range_idxs+1; 40 | 41 | % metadata on the problem 42 | problem.num_robots = double(problem.num_robots); 43 | problem.num_poses = double(problem.num_poses); 44 | problem.num_landmarks = double(problem.num_landmarks); 45 | problem.num_range_measurements = double(problem.num_range_measurements); 46 | problem.dim = double(mats.dim); 47 | 48 | 49 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/multihconj_legacy.m: -------------------------------------------------------------------------------- 1 | function b = multihconj_legacy(a, dim) 2 | %MULTIHCONJ Hermitian conjugating arrays of matrices. 3 | % 4 | % THIS ORIGINAL MULTIHCONJ IS NOW CALLED MULTIHCONJ_LEGACY 5 | % 6 | % B = MULTIHCONJ(A) is equivalent to B = MULTIHCONJ(A, DIM), where 7 | % DIM = 1. 8 | % 9 | % B = MULTIHCONJ(A, DIM) is equivalent to 10 | % B = PERMUTE(A, [1:DIM-1, DIM+1, DIM, DIM+2:NDIMS(A)]), where A is an 11 | % array containing N P-by-Q matrices along its dimensions DIM and DIM+1, 12 | % and B is an array containing the Q-by-P Hermitian conjugate (') of 13 | % those N matrices along the same dimensions. N = NUMEL(A) / (P*Q), i.e. 14 | % N is equal to the number of elements in A divided by the number of 15 | % elements in each matrix. 16 | % 17 | % 18 | % Example: 19 | % A 5-by-9-by-3-by-2 array may be considered to be a block array 20 | % containing ten 9-by-3 matrices along dimensions 2 and 3. In this 21 | % case, its size is so indicated: 5-by-(9-by-3)-by-2 or 5x(9x3)x2. 22 | % If A is ................ a 5x(9x3)x2 array of 9x3 matrices, 23 | % C = MULTIHCONJ(A, 2) is a 5x(3x9)x2 array of 3x9 matrices. 24 | % 25 | % See also MULTIHCONJ MULTITRANSP MULTIHERM. 26 | 27 | % This file is part of Manopt: www.manopt.org. 28 | % Original author: Hiroyuki Sato, April 27, 2015. 29 | % Contributors: 30 | % Change log: 31 | 32 | % Setting DIM if not supplied. 33 | if nargin == 1, dim = 1; end 34 | 35 | % Transposing 36 | b = multitransp_legacy(a, dim); 37 | 38 | %Conjugating 39 | b = conj(b); 40 | 41 | end 42 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/canGetGradient.m: -------------------------------------------------------------------------------- 1 | function candoit = canGetGradient(problem) 2 | % Checks whether the gradient can be computed for a problem structure. 3 | % 4 | % function candoit = canGetGradient(problem) 5 | % 6 | % Returns true if the gradient of the cost function can be computed given 7 | % the problem description, false otherwise. 8 | % 9 | % See also: canGetCost canGetDirectionalDerivative canGetHessian 10 | 11 | % This file is part of Manopt: www.manopt.org. 12 | % Original author: Nicolas Boumal, Dec. 30, 2012. 13 | % Contributors: 14 | % Change log: 15 | % 16 | % June 28, 2016 (NB): 17 | % Added support for getPartialGradient 18 | % 19 | % Nov. 1, 2016 (NB): 20 | % Added support for gradient from directional derivatives 21 | 22 | candoit = isfield(problem, 'grad') || isfield(problem, 'costgrad') || ... 23 | canGetEuclideanGradient(problem) || ... 24 | canGetPartialGradient(problem) || ... 25 | ... % Check if directional derivatives can be obtained, since 26 | ... % it is possible to compute the gradient from directional 27 | ... % derivatives (expensively). Here, it is not possible to 28 | ... % call canGetDirectionalDerivative, because that function 29 | ... % would then potentially call canGetGradient, thus 30 | ... % starting an infinite loop. As a result, we have some 31 | ... % code redundancy: the check below needs to be kept 32 | ... % equivalent to the check in canGetDirectionalDerivative. 33 | isfield(problem, 'diff'); 34 | 35 | end 36 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/hashmd5.m: -------------------------------------------------------------------------------- 1 | function h = hashmd5(inp) 2 | % Computes the MD5 hash of input data. 3 | % 4 | % function h = hashmd5(inp) 5 | % 6 | % Returns a string containing the MD5 hash of the input variable. The input 7 | % variable may be of any class that can be typecast to uint8 format, which 8 | % is fairly non-restrictive. 9 | 10 | % This file is part of Manopt: www.manopt.org. 11 | % This code is a stripped version of more general hashing code by 12 | % Michael Kleder, Nov 2005. 13 | % Change log: 14 | % 15 | % Aug. 8, 2013 (NB): 16 | % Made x a static (persistent) variable, in the hope it will speed 17 | % it up. Furthermore, the function is now Octave compatible. 18 | 19 | is_octave = exist('OCTAVE_VERSION', 'builtin'); 20 | 21 | persistent x; 22 | if isempty(x) && ~is_octave 23 | x = java.security.MessageDigest.getInstance('MD5'); 24 | end 25 | 26 | inp=inp(:); 27 | % Convert strings and logicals into uint8 format 28 | if ischar(inp) || islogical(inp) 29 | inp=uint8(inp); 30 | else % Convert everything else into uint8 format without loss of data 31 | inp=typecast(inp,'uint8'); 32 | end 33 | 34 | % Create hash 35 | if ~is_octave 36 | x.update(inp); 37 | h = typecast(x.digest, 'uint8'); 38 | h = dec2hex(h)'; 39 | % Remote possibility: all hash bytes < 128, so pad: 40 | if(size(h,1))==1 41 | h = [repmat('0',[1 size(h,2)]);h]; 42 | end 43 | h = lower(h(:)'); 44 | else 45 | h = md5sum(char(inp'), true); 46 | end 47 | 48 | end 49 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/cdiag.m: -------------------------------------------------------------------------------- 1 | function diagX = cdiag(X) 2 | % Extracts the diagonal elements of A. 3 | % 4 | % function diagX = cdiag(X) 5 | % 6 | % Returns the diagonal elements of A. The input A does not necessarily 7 | % to be a square matrix. The function supports both numeric arrays and 8 | % structs with fields real and imag. Note that diag currently does 9 | % not support dlarrays and cdiag can be seen as a backup function. 10 | % 11 | % See also: manoptADhelp 12 | 13 | % This file is part of Manopt: www.manopt.org. 14 | % Original author: Xiaowen Jiang, Aug. 31, 2021. 15 | % Contributors: Nicolas Boumal 16 | % Change log: 17 | 18 | if iscstruct(X) 19 | assert(length(size(X.real)) == 2, 'Input should be a 2-D array') 20 | m = size(X.real,1); 21 | n = size(X.real,2); 22 | realX = X.real; 23 | imagX = X.imag; 24 | if n >= m 25 | diagX.real = realX(1:m+1:m^2); 26 | diagX.imag = imagx(1:m+1:m^2); 27 | else 28 | diagX.real = realX(1:m+1:m*n-m+n); 29 | diagX.imag = imagX(1:m+1:m*n-m+n); 30 | end 31 | 32 | elseif isnumeric(X) 33 | assert(length(size(X)) == 2, 'Input should be a 2-D array') 34 | m = size(X,1); 35 | n = size(X,2); 36 | if n >= m 37 | diagX = X(1:m+1:m^2); 38 | else 39 | diagX = X(1:m+1:m*n-m+n); 40 | end 41 | 42 | else 43 | ME = MException('cdiag:inputError', ... 44 | 'Input does not have the expected format.'); 45 | throw(ME); 46 | end 47 | 48 | end 49 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/handle_light.m: -------------------------------------------------------------------------------- 1 | classdef handle_light < handle 2 | % Trick class to hide methods inherited from the handle class 3 | % when calling methods(myclass). 4 | % 5 | % Source: 6 | % http://stackoverflow.com/questions/6621850/is-it-possible-to-hide-the-methods-inherited-from-the-handle-class-in-matlab 7 | % Posted by sclarke81 on StackOverflow on Oct. 24, 2012. 8 | 9 | % This file is part of Manopt: www.manopt.org. 10 | % Original author: sclarke81, added April 3, 2013. 11 | % Contributors: 12 | % Change log: 13 | 14 | methods(Hidden) 15 | function lh = addlistener(varargin) 16 | lh = addlistener@handle(varargin{:}); 17 | end 18 | function notify(varargin) 19 | notify@handle(varargin{:}); 20 | end 21 | function delete(varargin) 22 | delete@handle(varargin{:}); 23 | end 24 | function Hmatch = findobj(varargin) 25 | Hmatch = findobj@handle(varargin{:}); 26 | end 27 | function p = findprop(varargin) 28 | p = findprop@handle(varargin{:}); 29 | end 30 | function TF = eq(varargin) 31 | TF = eq@handle(varargin{:}); 32 | end 33 | function TF = ne(varargin) 34 | TF = ne@handle(varargin{:}); 35 | end 36 | function TF = lt(varargin) 37 | TF = lt@handle(varargin{:}); 38 | end 39 | function TF = le(varargin) 40 | TF = le@handle(varargin{:}); 41 | end 42 | function TF = gt(varargin) 43 | TF = gt@handle(varargin{:}); 44 | end 45 | function TF = ge(varargin) 46 | TF = ge@handle(varargin{:}); 47 | end 48 | end 49 | 50 | end 51 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/tools/sylvester_nochecks.m: -------------------------------------------------------------------------------- 1 | function X = sylvester_nochecks(A, B, C) 2 | % Solve Sylvester equation without input checks. 3 | % 4 | % function X = sylvester_nochecks(A, B, C) 5 | % 6 | % Solves the Sylvester equation A*X + X*B = C, where A is an m-by-m matrix, 7 | % B is an n-by-n matrix, and X and C are two m-by-n matrices. 8 | % 9 | % This is a stripped-down version of Matlab's own sylvester function that 10 | % bypasses any input checks. This is significantly faster for small m and 11 | % n, which is often useful in Manopt. 12 | % 13 | % See also: sylvester lyapunov_symmetric 14 | 15 | % This file is part of Manopt: www.manopt.org. 16 | % Original author: Nicolas Boumal, July 19, 2018 17 | % Contributors: This is a modification of Matlab's built-in sylvester. 18 | % Change log: 19 | % July 30, 2020 (NB): 20 | % Changed call from builtin('_sylvester_tri', ...) to 21 | % matlab.internal.math.sylvester_tri(...), which seems necessary for 22 | % more recent versions of Matlab. 23 | % Also had to remove the second output: the 'unique' flag. 24 | 25 | flag = 'real'; 26 | if ~isreal(A) || ~isreal(B) || ~isreal(C) 27 | flag = 'complex'; 28 | end 29 | 30 | [QA, TA] = schur(A, flag); 31 | [QB, TB] = schur(B, flag); 32 | 33 | % Solve Sylvester Equation TA*Y + Y*TB = QA'*C*QB. 34 | Y = matlab.internal.math.sylvester_tri(TA, TB, QA'*C*QB, ... 35 | 'I', 'I', 'notransp'); 36 | % Use this call instead for older versions of Matlab: 37 | % [Y, info] = builtin('_sylvester_tri', TA, TB, QA'*C*QB); 38 | 39 | X = QA*Y*QB'; 40 | 41 | end 42 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/basic_examples_AD/realtest_AD2.m: -------------------------------------------------------------------------------- 1 | function realtest_AD2() 2 | % Test AD for a real optimization problem on a power manifold (cell) 3 | 4 | % Verify that Manopt was indeed added to the Matlab path. 5 | if isempty(which('spherefactory')) 6 | error(['You should first add Manopt to the Matlab path.\n' ... 7 | 'Please run importmanopt.']); 8 | end 9 | 10 | % Verify that the deep learning tool box was installed 11 | assert(exist('dlarray', 'file') == 2, ['Deep learning tool box is '... 12 | 'needed for automatic differentiation.\n Please install the'... 13 | 'latest version of the deep learning tool box and \nupgrade to Matlab'... 14 | ' R2021b if possible.']) 15 | 16 | % Generate the problem data. 17 | n = 100; 18 | A = randn(n); 19 | A = .5*(A+A'); 20 | 21 | % Create the power manifold 22 | S = spherefactory(n); 23 | problem.M = powermanifold(S,2); 24 | 25 | % Define the problem cost function 26 | problem.cost = @(X) -X{1}'*(A*X{2}); 27 | 28 | % Define the gradient and the hessian via automatic differentiation 29 | problem = manoptAD(problem); 30 | 31 | % Numerically check gradient and Hessian consistency. 32 | figure; 33 | checkgradient(problem); 34 | figure; 35 | checkhessian(problem); 36 | 37 | % Solve. 38 | [x, xcost, info] = trustregions(problem); %#ok 39 | 40 | % Test 41 | ground_truth = svd(A); 42 | distance = abs(ground_truth(1) - (-problem.cost(x))); 43 | fprintf('The distance between the ground truth and the solution is %e \n',distance); 44 | 45 | 46 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/rotations/randskew.m: -------------------------------------------------------------------------------- 1 | function S = randskew(n, N) 2 | % Generates random skew symmetric matrices with normal entries. 3 | % 4 | % function S = randskew(n) 5 | % function S = randskew(n, N) 6 | % 7 | % S is an n-by-n-by-N array where each slice S(:, :, i) for i = 1..N is a 8 | % random skew-symmetric matrix with upper triangular entries distributed 9 | % independently following a normal distribution (Gaussian, zero mean, unit 10 | % variance). 11 | % 12 | % By default, N = 1. 13 | % 14 | % See also: randrot randsym randskewh 15 | 16 | % This file is part of Manopt: www.manopt.org. 17 | % Original author: Nicolas Boumal, Sept. 25, 2012. 18 | % Contributors: 19 | % Change log: 20 | % June 19, 2019 (NB): 21 | % Now handles the case n = 1 properly. 22 | 23 | if nargin < 2 24 | N = 1; 25 | end 26 | 27 | if n == 1 28 | S = zeros(1, 1, N); 29 | return; 30 | end 31 | 32 | % Subindices of the (strictly) upper triangular entries of an n-by-n 33 | % matrix 34 | [I, J] = find(triu(ones(n), 1)); 35 | 36 | K = repmat(1:N, n*(n-1)/2, 1); 37 | 38 | % Indices of the strictly upper triangular entries of all N slices of 39 | % an n-by-n-by-N matrix 40 | L = sub2ind([n n N], repmat(I, N, 1), repmat(J, N, 1), K(:)); 41 | 42 | % Allocate memory for N random skew matrices of size n-by-n and 43 | % populate each upper triangular entry with a random number following a 44 | % normal distribution and copy them with opposite sign on the 45 | % corresponding lower triangular side. 46 | S = zeros(n, n, N); 47 | S(L) = randn(size(L)); 48 | S = S - multitransp(S); 49 | 50 | end 51 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op_laplace/constr_precond_inner.m: -------------------------------------------------------------------------------- 1 | function expB = constr_precond_inner( A, X, mu ) 2 | 3 | % TTeMPS Toolbox. 4 | % Michael Steinlechner, 2013-2016 5 | % Questions and contact: michael.steinlechner@epfl.ch 6 | % BSD 2-clause license, see LICENSE.txt 7 | 8 | n = size(A.L0, 1); 9 | sz = [X.rank(mu), X.size(mu), X.rank(mu+1)]; 10 | 11 | B1 = zeros( X.rank(mu) ); 12 | % calculate B1 part: 13 | for i = 1:mu-1 14 | % apply L to the i'th core 15 | tmp = X; 16 | tmp.U{i} = tensorprod_ttemps( tmp.U{i}, A.L0, 2 ); 17 | B1 = B1 + innerprod( X, tmp, 'LR', mu-1); 18 | end 19 | 20 | % calculate B2 part: 21 | B2 = A.L0; 22 | 23 | B3 = zeros( X.rank(mu+1) ); 24 | % calculate B3 part: 25 | for i = mu+1:A.order 26 | tmp = X; 27 | tmp.U{i} = tensorprod_ttemps( tmp.U{i}, A.L0, 2 ); 28 | B3 = B3 + innerprod( X, tmp, 'RL', mu+1); 29 | end 30 | 31 | [V1,e1] = eig(B1); 32 | e1 = diag(e1); 33 | [V3,e3] = eig(B3); 34 | e3 = diag(e3); 35 | 36 | lmin = min(e1) + min(A.E_L) + min(e3); 37 | lmax = max(e1) + max(A.E_L) + max(e3); 38 | 39 | R = lmax/lmin; 40 | 41 | [omega, alpha] = load_coefficients( R ); 42 | 43 | k = length(omega); 44 | omega = omega/lmin; 45 | alpha = alpha/lmin; 46 | 47 | expB = cell(3,k); 48 | 49 | for i = 1:k 50 | expB{1,i} = omega(i) * V1*diag( exp( -alpha(i)*e1 ))*V1'; % include omega in first part 51 | expB{2,i} = A.V_L*diag( exp( -alpha(i)*A.E_L ))*A.V_L'; 52 | expB{3,i} = V3*diag( exp( -alpha(i)*e3 ))*V3'; 53 | end 54 | end 55 | 56 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/basic_examples_AD/realtest_AD1.m: -------------------------------------------------------------------------------- 1 | function realtest_AD1() 2 | % Test AD for a real optimization problem on a product manifold (struct) 3 | 4 | % Verify that Manopt was indeed added to the Matlab path. 5 | if isempty(which('spherefactory')) 6 | error(['You should first add Manopt to the Matlab path.\n' ... 7 | 'Please run importmanopt.']); 8 | end 9 | 10 | % Verify that the deep learning tool box was installed 11 | assert(exist('dlarray', 'file') == 2, ['Deep learning tool box is '... 12 | 'needed for automatic differentiation.\n Please install the'... 13 | 'latest version of the deep learning tool box and \nupgrade to Matlab'... 14 | ' R2021b if possible.']) 15 | 16 | % Generate the problem data. 17 | n = 100; 18 | A = randn(n); 19 | A = .5*(A+A'); 20 | 21 | % Create the product manifold 22 | S = spherefactory(n); 23 | manifold.x = S; 24 | manifold.y = S; 25 | problem.M = productmanifold(manifold); 26 | 27 | % Define the problem cost function 28 | problem.cost = @(X) -X.x'*(A*X.y); 29 | 30 | % Define the gradient and the hessian via automatic differentiation 31 | problem = manoptAD(problem); 32 | 33 | % Numerically check gradient and Hessian consistency. 34 | figure; 35 | checkgradient(problem); 36 | figure; 37 | checkhessian(problem); 38 | 39 | % Solve. 40 | [x, xcost, info] = trustregions(problem); %#ok 41 | 42 | % Test 43 | ground_truth = svd(A); 44 | distance = abs(ground_truth(1) - (-problem.cost(x))); 45 | fprintf('The distance between the ground truth and the solution is %e \n',distance); 46 | 47 | 48 | end -------------------------------------------------------------------------------- /MATLAB/utils/viz_utils/plot_solution.m: -------------------------------------------------------------------------------- 1 | function plot_solution(X, problem_data, show_gt) 2 | 3 | % make sure that the height of X is dim 4 | dim = problem_data.dim; 5 | num_robots = problem_data.num_robots; 6 | if size(X, 1) ~= dim 7 | X = X'; 8 | end 9 | assert(size(X, 1) == dim); 10 | 11 | if show_gt 12 | gt_vals = align_solution_by_first_pose(problem_data.X_gt', problem_data); 13 | end 14 | Xvals = align_solution_by_first_pose(X, problem_data); 15 | 16 | % set up the figure for plotting 17 | figure(1) 18 | clf; 19 | hold on; 20 | 21 | %%%%%%%% PLOT POSES 22 | 23 | % iterate over robots and plot each trajectory with deterministic coloring 24 | colors = hsv(num_robots*2); 25 | for robot_idx = 1:num_robots 26 | robot_t_idxs = get_robot_t_idxs(problem_data, robot_idx); 27 | plot(Xvals(1, robot_t_idxs), Xvals(2, robot_t_idxs), 'Color', colors(robot_idx, :), 'LineWidth', 2); 28 | if show_gt 29 | plot(gt_vals(1, robot_t_idxs), gt_vals(2, robot_t_idxs),... 30 | "Color", colors(robot_idx+1, :), 'LineWidth', 1); 31 | end 32 | end 33 | 34 | %%%%%%%% PLOT LANDMARKS 35 | 36 | num_lands = problem_data.num_landmarks; 37 | if num_lands > 0 38 | landmarks = Xvals(:, problem_data.all_l_idxs); 39 | scatter(landmarks(1, :), landmarks(2, :), 20, 'bo') 40 | if show_gt 41 | scatter(gt_vals(1, problem_data.all_l_idxs), gt_vals(2, problem_data.all_l_idxs), 20, 'ro') 42 | end 43 | end 44 | 45 | vmax = max(X'); 46 | vmin = min(X'); 47 | 48 | xlim([vmin(1)-3, vmax(1)+3]); 49 | ylim([vmin(2)-3, vmax(2)+3]); 50 | 51 | % wait for user to close the figure 52 | waitfor(figure(1)); 53 | 54 | % remind to still try plotting ranges between objects, etc 55 | % warning('You should try plotting ranges between objects, etc') 56 | 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/solvers/trustregions/license for original GenRTR code.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2007,2012 Christopher G. Baker, Pierre-Antoine Absil, Kyle A. Gallivan 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the names of the contributors nor of their affiliated 12 | institutions may be used to endorse or promote products 13 | derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | For questions, please contact Chris Baker (chris@cgbaker.net) 27 | -------------------------------------------------------------------------------- /MATLAB/utils/io_utils/pose_graph/construct_translational_matrices.m: -------------------------------------------------------------------------------- 1 | function [T, Omega] = construct_translational_matrices(measurements, var_idx_mapping) 2 | %function [T, omega] = construct_translational_matrices(measurements) 3 | % 4 | % This function computes and returns the matrix T containing the raw 5 | % translational measurements (see eq. (24) in the paper) and the diagonal 6 | % matrix Omega containing the translational measurement precisions on its 7 | % main diagonal (see eq. (23) in the paper). 8 | 9 | % Copyright (C) 2016 by David M. Rosen 10 | 11 | D = length(measurements.t{1}); % D = dimension of SE(d) 12 | N = var_idx_mapping.pose_var_name_to_cnt.length; % N = number of nodes in the pose graph 13 | M = size(measurements.edges,1); % M = number of edges in the pose graph 14 | 15 | %Allocate storate for sparse matrix 16 | rows = zeros(1, D*M); 17 | cols = zeros(1, D*M); 18 | vals = zeros(1, D*M); 19 | 20 | omega = zeros(M, 1); 21 | out_nodes = cell2mat(values(var_idx_mapping.pose_var_name_to_cnt, measurements.edges(:, 1))); 22 | assert(all(out_nodes >= 1) && all(out_nodes <= N)); 23 | 24 | %Iterate over the measurements in the pose graph 25 | for e = 1:M 26 | 27 | %EXTRACT MEASUREMENT DATA 28 | k = out_nodes(e); %The index of the node at the tail of this edge 29 | 30 | tij = measurements.t{e}; %The translation corresponding to this observation 31 | 32 | omega(e) = measurements.tau{e}; %The precision for this translational observation 33 | 34 | 35 | %PROCESS MEASUREMENT DATA 36 | 37 | rows(D*(e-1) + 1 : D*(e-1) + D) = e*ones(1,D); 38 | cols(D*(e-1) + 1 : D*(e-1) + D) = [D*(k-1) + 1 : D*(k-1) + D]; 39 | vals(D*(e-1) + 1 : D*(e-1) + D) = -tij'; 40 | end 41 | 42 | T = sparse(rows, cols, vals, M, D*N); 43 | Omega = spdiags(omega, 0, M, M); 44 | end 45 | 46 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/functions_AD/ctrace.m: -------------------------------------------------------------------------------- 1 | function traceA = ctrace(A) 2 | % Computes the sum of diagonal elements of A. 3 | % 4 | % function traceA = ctrace(A) 5 | % 6 | % Returns the sum of diagonal elements of A. The input A does not have 7 | % to be a square matrix. The function supports both numeric arrays and 8 | % structs with fields real and imag. This file was created because trace 9 | % is not currently supported by dlarrays which we use for automatic 10 | % differentiation: ctrace is a backup function. 11 | % 12 | % See also: manoptADhelp 13 | 14 | % This file is part of Manopt: www.manopt.org. 15 | % Original author: Xiaowen Jiang, July 31, 2021. 16 | % Contributors: Nicolas Boumal 17 | % Change log: 18 | 19 | if iscstruct(A) 20 | assert(length(size(A.real)) == 2, 'Input should be a 2-D array') 21 | m = size(A.real, 1); 22 | n = size(A.real, 2); 23 | realA = A.real; 24 | imagA = A.imag; 25 | 26 | if n >= m 27 | traceA.real = sum(realA(1:m+1:m^2)); 28 | traceA.imag = sum(imagA(1:m+1:m^2)); 29 | else 30 | traceA.real = sum(realA(1:m+1:m*n-m+n)); 31 | traceA.imag = sum(imagA(1:m+1:m*n-m+n)); 32 | end 33 | 34 | elseif isnumeric(A) 35 | assert(length(size(A)) == 2, 'Input should be a 2-D array') 36 | m = size(A,1); 37 | n = size(A,2); 38 | if n >= m 39 | traceA = sum(A(1:m+1:m^2)); 40 | else 41 | traceA = sum(A(1:m+1:m*n-m+n)); 42 | end 43 | 44 | else 45 | ME = MException('ctrace:inputError', ... 46 | 'Input does not have the expected format.'); 47 | throw(ME); 48 | 49 | end 50 | 51 | end 52 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op/contract.m: -------------------------------------------------------------------------------- 1 | function res = contract( A, X, mu ) 2 | %CONTRACT Contraction of two TT/MPS tensors with inner TT/MPS operator. 3 | % Z = CONTRACT(A,X,IDX) contracts all coress of the two TT/MPS tensors X and Y=apply(A*X) except 4 | % core IDX. Result Z is a matrix of size 5 | % [ X.rank(IDX)*A.size_col(IDX)*X.rank(IDX+1), X.rank(IDX)*A.size_row(IDX),X.rank(IDX+1) ]. 6 | % 7 | % See also APPLY. 8 | 9 | % TTeMPS Toolbox. 10 | % Michael Steinlechner, 2013-2016 11 | % Questions and contact: michael.steinlechner@epfl.ch 12 | % BSD 2-clause license, see LICENSE.txt 13 | 14 | V = cell(1, A.order); 15 | 16 | Y = A.apply(X); 17 | if mu > 1 18 | left = innerprod( X, Y, 'LR', mu-1 ); 19 | left = reshape( left, [X.rank(mu), A.rank(mu), X.rank(mu)] ); 20 | left = reshape( permute(left, [1 3 2]), [X.rank(mu)*X.rank(mu), A.rank(mu)] ); 21 | res = left * reshape(A.U{mu}, [A.rank(mu), A.size_col(mu)*A.size_row(mu)*A.rank(mu+1)] ); 22 | else 23 | res = A.U{1}; 24 | end 25 | if mu < X.order 26 | right = innerprod( X, Y, 'RL', mu+1 ); 27 | right = reshape( right, [X.rank(mu+1), A.rank(mu+1), X.rank(mu+1)] ); 28 | right = matricize( right, 2); 29 | res = reshape( res, [ X.rank(mu)*X.rank(mu)*A.size_col(mu)*A.size_row(mu), A.rank(mu+1) ]); 30 | res = res * right; 31 | end 32 | 33 | res = reshape( res, [X.rank(mu), X.rank(mu), A.size_col(mu), A.size_row(mu), X.rank(mu+1), X.rank(mu+1) ] ); 34 | res = permute( res, [1 3 5 2 4 6] ); 35 | res = reshape( res, [X.rank(mu)*A.size_col(mu)*X.rank(mu+1), X.rank(mu)*A.size_row(mu)*X.rank(mu+1) ] ); 36 | 37 | end 38 | 39 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_op_laplace/constr_precond.m: -------------------------------------------------------------------------------- 1 | function P = constr_precond( A, k ) 2 | 3 | % TTeMPS Toolbox. 4 | % Michael Steinlechner, 2013-2016 5 | % Questions and contact: michael.steinlechner@epfl.ch 6 | % BSD 2-clause license, see LICENSE.txt 7 | 8 | d = A.order; 9 | ev = eig(A.L0); 10 | 11 | lmin = d*min(ev); 12 | lmax = d*max(ev); 13 | 14 | R = lmax/lmin 15 | 16 | if k == 3 17 | [omega, alpha] = load_coefficients( R ); 18 | 19 | elseif k == 7 20 | omega = [0.0133615547183825570028305575534521842940 0.0429728469424360175410925952177443321034 0.1143029399081515586560726591147663100401,... 21 | 0.2838881266934189482611071431161775535656 0.6622322841999484042811198458711174907876 1.4847175320092703810050463464342840325116,... 22 | 3.4859753729916252771962870138366952232900]; 23 | alpha = [0.0050213411684266507485648978019454613531 0.0312546410994290844202411500801774835168 0.1045970270084145620410366606112262388706,... 24 | 0.2920522758702768403556507270657505159761 0.7407504784499061527671195936939341208927 1.7609744335543204401530945069076494746696,... 25 | 4.0759036969145123916954953635638503328664]; 26 | else 27 | error('Unknown rank specified. Choose either k=3 or k=7'); 28 | end 29 | 30 | omega = omega/lmin; 31 | alpha = alpha/lmin; 32 | 33 | E = reshape( expm( -alpha(1) * A.L0), [1, A.size_row(1), A.size_col(1), 1]); 34 | P = omega(1)*TTeMPS_op( repmat({E},1,d) ); 35 | for i = 2:k 36 | E = reshape( expm( -alpha(i) * A.L0), [1, A.size_row(1), A.size_col(1), 1]); 37 | P = P + omega(i)*TTeMPS_op( repmat({E},1,d) ); 38 | end 39 | 40 | end 41 | -------------------------------------------------------------------------------- /MATLAB/manopt/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Manopt, a Matlab toolbox for optimization on manifolds, is copyright by 2 | Nicolas Boumal and is distributed under the terms of the GNU General Public 3 | License (GPL) version 3 (or later). See accompanying file or 4 | . 5 | 6 | In short, this means that everyone is free to use Manopt, to modify it and 7 | to redistribute it on a free basis. Manopt is not in the public domain; 8 | it is copyrighted and there are restrictions on its distribution (see the 9 | license). For example, you cannot integrate this version of Manopt (in full 10 | or in parts) in any closed-source software you plan to distribute 11 | (commercially or not). Please contact us for more information. 12 | 13 | Contact: 14 | http://www.manopt.org 15 | manopttoolbox@gmail.com 16 | 17 | The documentation of Manopt (the website) is copyright by Nicolas Boumal, 18 | all rights reserved. 19 | 20 | 21 | 22 | THIRD-PARTY CODE 23 | 24 | The following files contain third-party code or extensively rely on 25 | third-party code, and their specific license should be considered before 26 | modifying and/or redistributing them. The license information can be found 27 | either in the comments in the code or in a separate text file in the same 28 | directory as the Matlab files. 29 | 30 | /manopt/solvers/trustregions/trustregions.m 31 | /manopt/solvers/trustregions/tCG.m 32 | /manopt/tools/multitransp.m 33 | /manopt/tools/multiprod.m 34 | /manopt/tools/diagsum.m 35 | /manopt/tools/hashmd5.m 36 | 37 | 38 | 39 | CONTRIBUTIONS 40 | 41 | Contributions are licensed to the owners of Manopt under the Contributor 42 | License Agreement, see accompanying file . Be sure to check the 43 | header comments of Matlab files and look for the "original author" tag. 44 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS/orth_at.m: -------------------------------------------------------------------------------- 1 | function [x, R] = orth_at(x, pos, dir, apply) 2 | %ORTH_AT Orthogonalize single core. 3 | % X = ORTH_AT( X, POS, 'LEFT') left-orthogonalizes the core at position POS 4 | % and multiplies the corresponding R-factor with core POS+1. All other cores 5 | % are untouched. The modified tensor is returned. 6 | % 7 | % X = ORTH_AT( X, POS, 'RIGHT') right-orthogonalizes the core at position POS 8 | % and multiplies the corresponding R-factor with core POS-1. All other cores 9 | % are untouched. The modified tensor is returned. 10 | % 11 | % See also ORTHOGONALIZE. 12 | 13 | % TTeMPS Toolbox. 14 | % Michael Steinlechner, 2013-2016 15 | % Questions and contact: michael.steinlechner@epfl.ch 16 | % BSD 2-clause license, see LICENSE.txt 17 | 18 | if ~exist('apply', 'var') 19 | apply = true; 20 | end 21 | 22 | sz = size(x.U{pos}); 23 | 24 | if length(sz) == 2 25 | sz = [sz, 1]; 26 | end 27 | 28 | if strcmpi(dir, 'left') 29 | [Q, R] = qr_unique(unfold(x.U{pos}, 'left')); 30 | x.U{pos} = reshape(Q, [sz(1), sz(2), size(Q, 2)]); 31 | 32 | if apply 33 | x.U{pos + 1} = tensorprod_ttemps(x.U{pos + 1}, R, 1); 34 | end 35 | 36 | elseif strcmpi(dir, 'right') 37 | % mind the transpose as we want to orthonormalize rows 38 | [Q, R] = qr_unique(unfold(x.U{pos}, 'right')'); 39 | x.U{pos} = reshape(Q', [size(Q, 2), sz(2), sz(3)]); 40 | 41 | if apply 42 | x.U{pos - 1} = tensorprod_ttemps(x.U{pos - 1}, R, 3); 43 | end 44 | 45 | else 46 | error('Unknown direction specified. Choose either LEFT or RIGHT') 47 | end 48 | 49 | end 50 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/linearsystem/precond_rankOne.m: -------------------------------------------------------------------------------- 1 | % TTeMPS Toolbox. 2 | % Michael Steinlechner, 2013-2016 3 | % Questions and contact: michael.steinlechner@epfl.ch 4 | % BSD 2-clause license, see LICENSE.txt 5 | 6 | function [eta] = precond_rankOne( A, xi, xL, xR ) 7 | % A is a parameterdependent operator 8 | 9 | r = xL.rank; 10 | n = xL.size; 11 | d = xL.order; 12 | 13 | 14 | eta = xi; 15 | xi = tangent_to_TTeMPS( xi ); 16 | % % 1. STEP: Project right hand side 17 | % below is hard-coded version of 18 | % for ii=1:d 19 | % eta_partial_ii = TTeMPS_partial_project_overlap( xL, xR, xi, ii); 20 | % Y{ii} = eta_partial_ii.dU{ii}; 21 | % end 22 | 23 | % TODO, it seems that the left and right cell arrays consist of a lot of 24 | % identities and zeros. 25 | Y = cell(1,d); 26 | % precompute inner products 27 | left = innerprod( xL, xi, 'LR', d-1, true ); 28 | right = innerprod( xR, xi, 'RL', 2, true ); 29 | 30 | % contract to first core 31 | Y{1} = tensorprod_ttemps( xi.U{1}, right{2}, 3 ); 32 | % contract to first core 33 | for idx = 2:d-1 34 | res = tensorprod_ttemps( xi.U{idx}, left{idx-1}, 1 ); 35 | Y{idx} = tensorprod_ttemps( res, right{idx+1}, 3 ); 36 | end 37 | % contract to last core 38 | Y{d} = tensorprod_ttemps( xi.U{d}, left{d-1}, 1 ); 39 | 40 | 41 | tmp = (19.4*A.A{1}) \ unfold( Y{1}, 'left' ); 42 | eta.dU{1} = reshape( tmp, [r(1), n(1), r(2)]); 43 | 44 | x1 = matricize( xL.U{1}, 2); 45 | y = xL; 46 | y.U{1} = tensorize( (19.4*A.A{1})*x1, 2, [r(1), n(1), r(2)] ); 47 | B1 = innerprod( xL, y, 'LR', d-1, true); 48 | for i=2:d 49 | tmp = B1{i-1} \ unfold(Y{i}, 'right'); 50 | eta.dU{i} = reshape( tmp, [r(i), n(i), r(i+1)] ); 51 | end 52 | 53 | eta = TTeMPS_tangent_orth( xL, xR, eta ); 54 | 55 | end 56 | 57 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/completion/completion.m: -------------------------------------------------------------------------------- 1 | % Completion for Tensor train but without individual orthogonalization 2 | % (TTeMPS_tangent instead of TTeMPS_tangent_orth) 3 | % 4 | % WARNING: use completion_orth instead! 5 | 6 | % TTeMPS Toolbox. 7 | % Michael Steinlechner, 2013-2016 8 | % Questions and contact: michael.steinlechner@epfl.ch 9 | % BSD 2-clause license, see LICENSE.txt 10 | function [X,cost,test] = completion( A_Omega, Omega, A_Gamma, Gamma, X, maxiter, cg ) 11 | 12 | n = X.size; 13 | r = X.rank; 14 | 15 | cost = zeros(maxiter,1); 16 | test = zeros(maxiter,1); 17 | for i = 1:maxiter 18 | grad = euclidgrad(A_Omega, X, Omega); 19 | xi = TTeMPS_tangent(X, grad, Omega); 20 | %xi_full = full(xi, X); 21 | ip_xi_xi = innerprod(xi, xi, X); 22 | if (i == 1) || (~cg) 23 | eta = -xi; 24 | else 25 | ip_xitrans_xi = innerprod( xi_trans, xi, X); 26 | theta = ip_xitrans_xi / ip_xi_xi; 27 | if theta >= 0.3 28 | eta = -xi; 29 | disp('steepest descent step') 30 | else 31 | disp('CG step') 32 | beta = ip_xi_xi/ip_xi_xi_old; 33 | eta = -xi + beta*TTeMPS_tangent( X, eta ); 34 | end 35 | end 36 | 37 | %line search 38 | eta_Omega = at_Omega( eta, Omega, X); 39 | alpha = -(eta_Omega'*grad) / norm(eta_Omega)^2; 40 | 41 | X = tangentAdd( eta, alpha, X, true); 42 | X = orthogonalize( X, X.order); 43 | cost(i) = func(A_Omega, X, Omega); 44 | test(i) = func(A_Gamma, X, Gamma); 45 | ip_xi_xi_old = ip_xi_xi; 46 | xi_trans = TTeMPS_tangent( X, xi); 47 | end 48 | 49 | end 50 | 51 | 52 | function res = func(A_Omega, X, Omega) 53 | res = 0.5*norm( A_Omega - X(Omega) )^2; 54 | end 55 | 56 | function res = euclidgrad(A_Omega, X, Omega) 57 | res = X(Omega) - A_Omega; 58 | end 59 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/@TTeMPS_block/innerprod.m: -------------------------------------------------------------------------------- 1 | function res = innerprod( x, y, dir, upto ) 2 | %INNERPROD Inner product between two TT/MPS tensors. 3 | % innerprod(X,Y) computes the inner product between the TT/MPS tensors X and Y. 4 | % Assumes that the first rank of both tensors, X.rank(1) and Y.rank(1), is 1. 5 | % The last rank may be different from 1, resulting in a matrix of size 6 | % [X.rank(end), Y.rank(end)]. 7 | % 8 | % See also NORM 9 | 10 | % TTeMPS Toolbox. 11 | % Michael Steinlechner, 2013-2016 12 | % Questions and contact: michael.steinlechner@epfl.ch 13 | % BSD 2-clause license, see LICENSE.txt 14 | 15 | if ~exist( 'dir', 'var' ) 16 | dir = 'LR'; 17 | end 18 | if ~exist( 'upto', 'var' ) 19 | if strcmpi( dir, 'LR') 20 | upto = x.order; 21 | else 22 | upto = 1; 23 | end 24 | end 25 | 26 | % Left-to-Right procedure 27 | if strcmpi( dir, 'LR') 28 | 29 | res = unfold( x.U{1}, 'left')' * unfold( y.U{1}, 'left'); 30 | 31 | for i = 2:upto 32 | tmp = tensorprod_ttemps( x.U{i}, res', 1); 33 | res = unfold( tmp, 'left')' * unfold( y.U{i}, 'left'); 34 | end 35 | 36 | % Right-to-Left procedure 37 | elseif strcmpi( dir, 'RL') 38 | d = x.order; 39 | res = conj(unfold( x.U{d}, 'right')) * unfold( y.U{d}, 'right').'; 40 | 41 | for i = d-1:-1:upto 42 | tmp = tensorprod_ttemps( x.U{i}, res', 3); 43 | res = conj(unfold( tmp, 'right')) * unfold( y.U{i}, 'right').'; 44 | end 45 | 46 | else 47 | error('Unknown direction specified. Choose either LR (default) or RL') 48 | end 49 | 50 | end 51 | 52 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1/algorithms/linearsystem/alsLinsolve.m: -------------------------------------------------------------------------------- 1 | % TTeMPS Toolbox. 2 | % Michael Steinlechner, 2013-2016 3 | % Questions and contact: michael.steinlechner@epfl.ch 4 | % BSD 2-clause license, see LICENSE.txt 5 | function [X, residuum, cost] = alsLinsolve( L, F, X, opts ) 6 | 7 | % set default opts 8 | if ~exist( 'opts', 'var'); opts = struct(); end 9 | if ~isfield( opts, 'nSweeps'); opts.nSweeps = 4; end 10 | 11 | d = X.order; 12 | n = X.size; 13 | 14 | 15 | normF = norm(F); 16 | cost = cost_function( L, X, F ); 17 | residuum = norm( apply(L, X) - F ) / normF; 18 | 19 | for sweep = 1:opts.nSweeps 20 | X = orthogonalize(X, 1); 21 | for i = 1:d-1 22 | disp( ['Current core: ', num2str(i)] ) 23 | 24 | Li = contract( L, X, i ); 25 | Fi = contract( X, F, i ); 26 | 27 | Ui = Li \ Fi(:); 28 | X.U{i} = reshape( Ui, size(X.U{i}) ); 29 | X = orth_at( X, i, 'left', true ); 30 | 31 | residuum = [residuum; norm( apply(L, X) - F ) / normF]; 32 | cost = [cost; cost_function( L, X, F )]; 33 | end 34 | for i = d:-1:2 35 | disp( ['Current core: ', num2str(i)] ) 36 | 37 | Li = contract( L, X, i ); 38 | Fi = contract( X, F, i ); 39 | 40 | Ui = Li \ Fi(:); 41 | X.U{i} = reshape( Ui, size(X.U{i}) ); 42 | X = orth_at( X, i, 'right', true ); 43 | residuum = [residuum; norm( apply(L, X) - F ) / normF]; 44 | cost = [cost; cost_function( L, X, F )]; 45 | end 46 | 47 | end 48 | 49 | 50 | end 51 | 52 | function res = cost_function( L, X, F ) 53 | res = 0.5*innerprod( X, apply(L, X) ) - innerprod( X, F ); 54 | end 55 | 56 | function res = euclid_grad( L, X, F ) 57 | res = apply(L, X) - F; 58 | end 59 | 60 | -------------------------------------------------------------------------------- /MATLAB/utils/math_utils/check_value_is_valid.m: -------------------------------------------------------------------------------- 1 | function is_valid = check_value_is_valid(problem, X) 2 | % innocent until proven guilty 3 | is_valid = true; 4 | 5 | % if X is tall and skinny, transpose it 6 | if size(X, 1) > size(X, 2) 7 | X = X'; 8 | end 9 | 10 | % for each pose, check if the rotation is valid (i.e., R.T * R = I and 11 | % det(R) = 1) 12 | all_rot_idxs = problem.all_R_idxs; 13 | dim = problem.dim; 14 | num_poses = problem.num_poses; 15 | for i = 1:num_poses 16 | rot_i_idx_start = dim * (i-1) + 1; 17 | rot_i_idx_end = (dim * i); 18 | rot_start_idx = all_rot_idxs(rot_i_idx_start); 19 | rot_end_idx = all_rot_idxs(rot_i_idx_end); 20 | R = X(:, rot_start_idx:rot_end_idx); 21 | assert (size(R, 1) == dim); 22 | assert (size(R, 2) == dim); 23 | 24 | det_R = det(R); 25 | if (abs(abs(det_R) - 1) > 1e-6) 26 | is_valid = false; 27 | fprintf('rot %d: det(R) is not 1 or -1, det(R) = %f\n',i, det_R); 28 | return; 29 | end 30 | 31 | R_T = R'; 32 | R_T_R = R_T * R; 33 | if (norm(R_T_R - eye(dim)) > 1e-6) 34 | is_valid = false; 35 | fprintf('rot %d: R.T * R is not I, residual = %f\n',i, norm(R_T_R - eye(dim))); 36 | disp(R_T_R); 37 | return; 38 | end 39 | 40 | end 41 | 42 | % for each distance variable make sure it is a unit vector 43 | all_dist_idxs = problem.all_d_idxs; 44 | for i = 1:length(all_dist_idxs) 45 | dist_i_idx = all_dist_idxs(i); 46 | dist_i = X(:, dist_i_idx); 47 | if (abs(norm(dist_i) - 1) > 1e-6) 48 | is_valid = false; 49 | fprintf('dist %d: norm is not 1, norm = %f',i, norm(dist_i)); 50 | return; 51 | end 52 | end 53 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/manifolds/rotations/randrot.m: -------------------------------------------------------------------------------- 1 | function Q = randrot(n, N) 2 | % Generates uniformly random rotation matrices. 3 | % 4 | % function Q = randrot(n, N) 5 | % 6 | % Q is an n-by-n-by-N array such that each slice Q(:, :, i) is a random 7 | % orthogonal matrix of size n of determinant +1 (i.e., a matrix in SO(n)), 8 | % sampled from the Haar measure (uniform distribution). 9 | % 10 | % By default, N = 1. 11 | % 12 | % Complexity: N times O(n^3). 13 | % Theory in Diaconis and Shahshahani 1987 for the uniformity on O(n); 14 | % With details in Mezzadri 2007, 15 | % "How to generate random matrices from the classical compact groups." 16 | % 17 | % To ensure matrices in SO(n), we permute the two first columns when 18 | % the determinant is -1. 19 | % 20 | % See also: randskew qr_unique randunitary 21 | 22 | % This file is part of Manopt: www.manopt.org. 23 | % Original author: Nicolas Boumal, Sept. 25, 2012. 24 | % Contributors: 25 | % Change log: 26 | % June 18, 2019 (NB) 27 | % Now generating all initial random matrices in one shot (which 28 | % should be more efficient) and calling qr_unique. 29 | 30 | 31 | if nargin < 2 32 | N = 1; 33 | end 34 | 35 | if n == 1 36 | Q = ones(1, 1, N); 37 | return; 38 | end 39 | 40 | % Generated as such, Q is uniformly distributed over O(n): the group 41 | % of orthogonal matrices; see Mezzadri 2007. 42 | Q = qr_unique(randn(n, n, N)); 43 | 44 | for k = 1 : N 45 | 46 | % If a slice of Q is in O(n) but not in SO(n), we permute its two 47 | % first columns to negate its determinant. This ensures the new 48 | % slice is in SO(n), uniformly distributed. 49 | if det(Q(:, :, k)) < 0 50 | Q(:, [1 2], k) = Q(:, [2 1], k); 51 | end 52 | 53 | end 54 | 55 | end 56 | -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/autodiff/basic_examples_AD/realtest_AD3.m: -------------------------------------------------------------------------------- 1 | function realtest_AD3() 2 | % Test AD for a real optimization problem on a manifold which is stored in 3 | % a particular data structure which is recursively defined by a struct, an 4 | % array and a cell. 5 | 6 | % Verify that Manopt was indeed added to the Matlab path. 7 | if isempty(which('spherefactory')) 8 | error(['You should first add Manopt to the Matlab path.\n' ... 9 | 'Please run importmanopt.']); 10 | end 11 | 12 | % Verify that the deep learning tool box was installed 13 | assert(exist('dlarray', 'file') == 2, ['Deep learning tool box is '... 14 | 'needed for automatic differentiation.\n Please install the'... 15 | 'latest version of the deep learning tool box and \nupgrade to Matlab'... 16 | ' R2021b if possible.']) 17 | 18 | % Generate the problem data. 19 | n = 100; 20 | A = randn(n); 21 | A = .5*(A+A'); 22 | 23 | % Create the manifold 24 | S = spherefactory(n); 25 | P = powermanifold(S,1); % cell 26 | X.x = S; 27 | X.y = P; 28 | problem.M = productmanifold(X); % struct 29 | 30 | % Define the problem cost function 31 | problem.cost = @(X) -X.x'*(A*X.y{1}); 32 | 33 | % Define the gradient and the hessian via automatic differentiation 34 | problem = manoptAD(problem); 35 | 36 | % Numerically check gradient and Hessian consistency. 37 | figure; 38 | checkgradient(problem); 39 | figure; 40 | checkhessian(problem); 41 | 42 | % Solve. 43 | [x, xcost, info] = trustregions(problem); %#ok 44 | 45 | % Test 46 | ground_truth = svd(A); 47 | distance = abs(ground_truth(1) - (-problem.cost(x))); 48 | fprintf('The distance between the ground truth and the solution is %e \n',distance); 49 | 50 | 51 | end -------------------------------------------------------------------------------- /MATLAB/manopt/manopt/core/getGlobalDefaults.m: -------------------------------------------------------------------------------- 1 | function opts = getGlobalDefaults() 2 | % Returns a structure with default option values for Manopt. 3 | % 4 | % function opts = getGlobalDefaults() 5 | % 6 | % Returns a structure opts containing the global default options such as 7 | % verbosity level etc. Typically, global defaults are overwritten by solver 8 | % defaults, which are in turn overwritten by user-specified options. 9 | % See the online Manopt documentation for details on options. 10 | % 11 | % See also: mergeOptions 12 | 13 | % This file is part of Manopt: www.manopt.org. 14 | % Original author: Nicolas Boumal, Dec. 30, 2012. 15 | % Contributors: 16 | % Change log: 17 | % 18 | % Aug. 2, 2018 (NB): 19 | % Changed default storedepth to 2 from 20, since solvers should now 20 | % use storedb.erase() to keep the cache lean. 21 | 22 | 23 | % There should be no reason to modify this file. 24 | % For better compatibility with future Manopt versions, 25 | % use the options structure of solvers. 26 | % 27 | % Really: don't modify it. 28 | 29 | 30 | % Verbosity level: 0 is no output at all. The higher the verbosity, the 31 | % more info is printed / displayed during solver execution. 32 | opts.verbosity = 3; 33 | 34 | % If debug is set to true, additional computations may be performed and 35 | % debugging information is outputed during solver execution. 36 | opts.debug = false; 37 | 38 | % Maximum number of store structures to store. If set to 0, caching 39 | % capabilities are not disabled, but the cache will be emptied at each 40 | % iteration of iterative solvers (more specifically: every time the 41 | % solver calls to purge the storedb). 42 | opts.storedepth = 2; 43 | 44 | % Maximum amount of time a solver may execute, in seconds. 45 | opts.maxtime = inf; 46 | 47 | end 48 | --------------------------------------------------------------------------------