├── .gitignore ├── ComplexEdge_Vision_Example_Small.m ├── Data ├── 2D_Pose_Graph │ └── INTEL_P.g2o ├── 2D_RGBD_Victoria_Park │ ├── CovMatrixInv_DLR.mat │ ├── CovMatrixInv_VicPark_6898_loops.mat │ ├── Xstate_odo.mat │ ├── Zstate_DLR.mat │ └── Zstate_VicPark_6898_loops.mat └── RGBD_K │ ├── 4260.txt │ ├── 4265.txt │ ├── 4270.txt │ └── feature_post_list.mat ├── Example_Junwang.m ├── Example_VINS.m ├── Example_VINS_visual.m ├── Example_Vic_incre.m ├── Example_VictoriaPark.m ├── Factor ├── GetEdgeTypeDimension.m ├── GetFactorX_format.m ├── GetNodeTypeDimension.m ├── IMU_Factor.m ├── IMUbias_Factor.m ├── LinearVisionNoLandmark_Factor.m ├── LinearVision_Factor.m ├── PBA_factor │ ├── DirectProject.m │ ├── NormalFromUV.m │ ├── VisionPBA_AssAnchor_Factor.m │ ├── VisionPBA_Factor.m │ ├── VisionPBA_MainAnchor_Factor.m │ ├── VisionTestPBA_AssAnchor_Factor.m │ ├── VisionTestPBA_Factor.m │ ├── VisionTestPBA_MainAnchor_Factor.m │ └── dDirectProject.m ├── Prediction_IMU.m ├── PriorPose2_Factor.m ├── PriorPose3_Factor.m ├── PriorVelAndbias_Factor.m ├── RGBD_2D_Factor.m ├── RGBD_Factor.m ├── RelativePose2_Factor.m ├── RelativePose3_Factor.m ├── RelativePose3_consX_Factor.m ├── SetNodeDefaultValue.m ├── VisionNoLandmark_Factor.m ├── VisionPoseFixed_Factor.m ├── VisionTestPoseFixed_Factor.m ├── VisionTest_Factor.m ├── Vision_Factor.m └── update_state.m ├── Geometry ├── Cal_triangulate.m ├── Fn_GetLandmarkCoordinate.m ├── Fn_LandmarkManager.m ├── Fn_vectorLandmarkP3.m ├── GetParallexFromTwoPoses.m └── logmap_parallax.m ├── Math ├── Jaco_r_se3_inverse.m ├── Jacobian_Lie.m ├── Jacobian_Lie_inverse.m ├── Jr.m ├── Jr_inverse.m ├── Kr.m ├── PBA_Jacobian.m ├── SE23.m ├── TestJacobian.m ├── Test_Jr.m ├── V_so2.m ├── adjoint.m ├── computeA.m ├── jaco_r.m ├── jacor_inverse.m ├── se23_log.m ├── se3_exp.m ├── se3_group.m ├── se3_log.m ├── skew.m ├── so2_exp.m ├── so3_exp.m ├── so3_hatinv.m ├── so3_log.m ├── special_add_left.m └── special_add_right.m ├── NewExample_VINS_visual.m ├── PBAexample ├── AddInitialGuessForPoses.m ├── GeneratePBALandmarkManager.m ├── MakeAnchorAndGenerateInitial.m ├── MakeGraphPBA.m ├── PBA_Example.m ├── VINS3.mat └── VINS_cubic3.mat ├── PBAexample_2 ├── AddInitialGuessForPoses.m ├── GeneratePBALandmarkManager.m ├── MakeAnchorAndGenerateInitial.m ├── MakeGraphPBA.m ├── PBA_Example.m ├── TestVision_Example_Small.m └── newVINS.mat ├── PlotTrajectory.m ├── PlotTrajectory_Ground.m ├── PoseFixed_NewExample_VINS_visual.m ├── README.md ├── SecondExample_VINS_visual.m ├── TestVision_Example_Small.m ├── Third_Example_VINS_visual.m ├── VINS2.mat ├── VINS3.mat ├── VINS4.mat ├── VINS5.mat ├── VINS_DataGenerator ├── A_ProduceData │ ├── Fun_IMUdataIJ.m │ ├── Fun_PreIntegration_bias.m │ ├── Fun_generateFeatures.m │ ├── Fun_getData.m │ ├── Fun_initilization.m │ └── SimulatorVehicle.m ├── Factors │ ├── Factor_IMU.m │ ├── Factor_Prior.m │ └── Factor_Vision.m ├── Fun_DoPreintegration.m ├── Fun_InitialEstimate.m ├── Fun_Optimization_GN.m ├── Fun_ProduceData.m ├── Integration.zip ├── MathOperation │ ├── EularAngle.m │ ├── J_rota.m │ ├── Jr.m │ ├── Jr_inverse.m │ ├── Kr.m │ ├── Update_fullstate.m │ ├── ad_G.m │ ├── ad_G_inverse.m │ ├── fun_x2X.m │ ├── jaco_r.m │ ├── jacor_inverse.m │ ├── skew.m │ ├── so3_exp.m │ ├── so3_hatinv.m │ ├── so3_log.m │ ├── test.m │ ├── update_Features.m │ └── update_robotstate.m ├── OptimizationProcess │ ├── CreatSparseJacobian_rowcol_index.m │ ├── Fun_GetJacobianError.m │ └── Untitled.m ├── Untitled.m ├── main.m ├── matlab.mat ├── matlab2.mat ├── rk4_manifold_mod.m ├── rk4_manifold_mod2.m ├── rk4_manifold_mod3.m ├── rk4_manifold_mod4.m ├── so3_exp.m └── testInte.m ├── VINS_cubic.mat ├── VINS_cubic2.mat ├── VINS_cubic3.mat ├── Vision_Example_Small.m ├── auxilliary ├── GenerateUV_randn.m └── UVD2LocalXYZ.m ├── doc ├── Graph-Optimization-Matlab.pdf └── Optimization_on_manifold_v1.pdf └── g2o_files ├── AddComplexEdge.m ├── AddNormalEdge.m ├── AddOneNode.m ├── AddUnaryEdge.m ├── Cal_ObjValue.m ├── Cal_ObjValue_New.m ├── Fn_processNodeTable.m ├── GenerateIndexVector.m ├── GetInformationMatrix.m ├── InitializeGraph.m ├── PerformGO.m ├── PerformGO_DL.m ├── PerformGO_DLBackup.m ├── PerformGO_DLnew.m ├── PerformGO_DLnew_sch.m ├── PerformGO_LM.m ├── calculate_inverse.m ├── calculate_inverse_new.m ├── classifyNodeType.m ├── compute_DogLeg.m └── update_radius.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.md -------------------------------------------------------------------------------- /ComplexEdge_Vision_Example_Small.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | 8 | 9 | pose0=[eye(3) zeros(3,1)]; 10 | pose1=[expm(skew([ 0.1 ;-0.05; 0.2 ])) [ 2; -2 ;-1] ]; 11 | pose2=[expm(skew([ -0.15 ; 0.05; -0.2 ])) [ -1.5; 2.3 ;-1.2] ]; 12 | pose3=[expm(skew([ 0.4 ; -0.1; 0.35 ])) [ -3; -2.7 ;-2] ]; 13 | 14 | 15 | f=cell(1,10); 16 | f{1} = [ 1; 2 ;9 ]; 17 | f{2} = [ -1; 2 ;7]; 18 | f{3} = [ -2; 1 ; 11 ]; 19 | f{4} = [ -1.5; 2.4 ; 7.6 ]; 20 | f{5}= [ 3; 2 ; 9.4 ]; 21 | f{6}= [ -4; 4 ; 14 ]; 22 | f{7}= [ 4; -6 ; 10 ]; 23 | f{8}= [ 5; -2 ; 9.5 ]; 24 | f{9}= [ 0; 0 ; 4 ]; 25 | f{10}= [ 0.2; 0.5 ; 5 ]; 26 | 27 | fprintf('Ground Truth of landmarks\n') 28 | 29 | 30 | 31 | 32 | 33 | 34 | [ Graph ] = InitializeGraph; 35 | Graph.Fixed.IDname.pose0 = 1; 36 | %Graph.Fixed.IDname.pose1 = 1; 37 | 38 | 39 | 40 | k=0; 41 | for i=1:10 42 | fea = f{i}; 43 | 44 | [ UV_i_0 ] = GenerateUV_randn( pose0, f{i} ); 45 | 46 | R = pose0(1:3,1:3); p = pose0(1:3,4); d = norm(R'*( fea - p )); 47 | Measurement_i_0.value = UV_i_0; 48 | NodeArray=cell(2,2); 49 | NodeArray{1,1}='Pose3';NodeArray{1,2}='pose0'; 50 | NodeArray{2,1}='Landmark3';NodeArray{2,2}=['landmark' num2str(i)]; 51 | 52 | if k 53 | Measurement_i_0.inf = eye(2); %eye(2)/(d^2)*525^2; 54 | [ Graph ] = AddComplexEdge( Graph, 'LinearVision_Factor', NodeArray, Measurement_i_0 ); 55 | else 56 | Measurement_i_0.inf = eye(2); 57 | [ Graph ] = AddComplexEdge( Graph, 'Vision_Factor', NodeArray, Measurement_i_0 ); 58 | end 59 | 60 | [ UV_i_1 ] = GenerateUV_randn( pose1, f{i} ); 61 | R = pose1(1:3,1:3); p = pose1(1:3,4); d = norm(R'*( fea - p )); 62 | Measurement_i_1.value = UV_i_1; 63 | Measurement_i_1.inf = eye(2)/(d^2)*525^2; 64 | NodeArray{1,2}='pose1'; 65 | if k 66 | Measurement_i_1.inf = eye(2); %eye(2)/(d^2)*525^2; 67 | [ Graph ] = AddComplexEdge( Graph, 'LinearVision_Factor', NodeArray, Measurement_i_1 ); 68 | else 69 | Measurement_i_1.inf = eye(2); 70 | [ Graph ] = AddComplexEdge( Graph, 'Vision_Factor', NodeArray, Measurement_i_1 ); 71 | end 72 | 73 | [ UV_i_2 ] = GenerateUV_randn( pose2, f{i} ); 74 | R = pose2(1:3,1:3); p = pose2(1:3,4); d = norm(R'*( fea - p )); 75 | Measurement_i_2.value = UV_i_2; 76 | Measurement_i_2.inf = eye(2)/(d^2); 77 | NodeArray{1,2}='pose2'; 78 | if k 79 | Measurement_i_2.inf = eye(2); %eye(2)/(d^2)*525^2; 80 | [ Graph ] = AddComplexEdge( Graph, 'LinearVision_Factor', NodeArray, Measurement_i_2 ); 81 | else 82 | Measurement_i_2.inf = eye(2); 83 | [ Graph ] = AddComplexEdge( Graph, 'Vision_Factor', NodeArray, Measurement_i_2 ); 84 | end 85 | 86 | [ UV_i_3 ] = GenerateUV_randn( pose3, f{i} ); 87 | R = pose3(1:3,1:3); p = pose3(1:3,4); d = norm(R'*( fea - p )); 88 | Measurement_i_3.value = UV_i_3; 89 | Measurement_i_3.inf = eye(2)/(d^2); 90 | NodeArray{1,2}='pose3'; 91 | if k 92 | Measurement_i_3.inf = eye(2); % eye(2)/(d^2)*525^2; 93 | [ Graph ] = AddComplexEdge( Graph, 'LinearVision_Factor', NodeArray, Measurement_i_3 ); 94 | else 95 | Measurement_i_3.inf = eye(2); 96 | [ Graph ] = AddComplexEdge( Graph, 'Vision_Factor', NodeArray, Measurement_i_3 ); 97 | end 98 | 99 | end 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | %%% Set initial guess via ground truth+noise 108 | Graph.Nodes.Pose3.Values.pose0=pose0; 109 | noise1 = [ expm(skew(randn(3,1)*0)) randn(3,1)*0]; 110 | Graph.Nodes.Pose3.Values.pose1=se3_group(pose1, noise1 ) ; 111 | noise2 = [ expm(skew(randn(3,1)*0.1)) randn(3,1)*0.2]; 112 | Graph.Nodes.Pose3.Values.pose2=se3_group(pose2, noise2 ) ; 113 | noise3 = [ expm(skew(randn(3,1)*0.1)) randn(3,1)*0.2]; 114 | Graph.Nodes.Pose3.Values.pose3=se3_group(pose3, noise3 ) ; 115 | 116 | 117 | Y=cell(10,1); 118 | 119 | 120 | for i=1:10 121 | G_i=f{i}; 122 | Graph.Nodes.Landmark3.Values.(['landmark' num2str(i)])=f{i}+1*randn(3,1); 123 | X_i=Graph.Nodes.Landmark3.Values.(['landmark' num2str(i)]); 124 | Y{i}=X_i; 125 | end 126 | %Graph.Nodes.Landmark3.Values.landmark1=[1.02;2.1;9.1]; 127 | %Y{1} = [1;1;1]; 128 | %%% Set initial guess 129 | %[ Graph ] = PerformGO( Graph ); 130 | tic 131 | [ Graph ] = PerformGO_DL( Graph ); 132 | toc 133 | %[ Graph ] = PerformGO( Graph ); 134 | 135 | 136 | 137 | for i=1:10 138 | G_i=f{i}; 139 | X_i=Graph.Nodes.Landmark3.Values.(['landmark' num2str(i)]); 140 | Y_i=Y{i}; 141 | fprintf('%d, %f, %f, %f \n', i, G_i(1),X_i(1),Y_i(1) ) 142 | fprintf('%d, %f, %f, %f \n', i, G_i(2),X_i(2),Y_i(2) ) 143 | fprintf('%d, %f, %f, %f \n', i, G_i(3),X_i(3),Y_i(3) ) 144 | end 145 | 146 | 147 | -------------------------------------------------------------------------------- /Data/2D_RGBD_Victoria_Park/CovMatrixInv_DLR.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/Data/2D_RGBD_Victoria_Park/CovMatrixInv_DLR.mat -------------------------------------------------------------------------------- /Data/2D_RGBD_Victoria_Park/CovMatrixInv_VicPark_6898_loops.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/Data/2D_RGBD_Victoria_Park/CovMatrixInv_VicPark_6898_loops.mat -------------------------------------------------------------------------------- /Data/2D_RGBD_Victoria_Park/Xstate_odo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/Data/2D_RGBD_Victoria_Park/Xstate_odo.mat -------------------------------------------------------------------------------- /Data/2D_RGBD_Victoria_Park/Zstate_DLR.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/Data/2D_RGBD_Victoria_Park/Zstate_DLR.mat -------------------------------------------------------------------------------- /Data/2D_RGBD_Victoria_Park/Zstate_VicPark_6898_loops.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/Data/2D_RGBD_Victoria_Park/Zstate_VicPark_6898_loops.mat -------------------------------------------------------------------------------- /Data/RGBD_K/4260.txt: -------------------------------------------------------------------------------- 1 | TVector 2 | 1.069093 3 | 1.390157 4 | 1.895715 5 | 6 | RMatrix 7 | 0.860109 -0.014255 -0.509910 8 | -0.000869 -0.999649 0.026480 9 | -0.510109 -0.022333 -0.859820 10 | 11 | Camera Intrinsics: focal height width 12 | 525.000000 480 640 -------------------------------------------------------------------------------- /Data/RGBD_K/4265.txt: -------------------------------------------------------------------------------- 1 | TVector 2 | 1.095943 3 | 1.374461 4 | 1.921930 5 | 6 | RMatrix 7 | 0.853790 -0.007297 -0.520566 8 | 0.017675 -0.998919 0.042993 9 | -0.520317 -0.045908 -0.852738 10 | 11 | Camera Intrinsics: focal height width 12 | 525.000000 480 640 -------------------------------------------------------------------------------- /Data/RGBD_K/4270.txt: -------------------------------------------------------------------------------- 1 | TVector 2 | 1.112855 3 | 1.361110 4 | 1.943661 5 | 6 | RMatrix 7 | 0.848725 -0.009615 -0.528747 8 | 0.017975 -0.998732 0.047014 9 | -0.528529 -0.049406 -0.847476 10 | 11 | Camera Intrinsics: focal height width 12 | 525.000000 480 640 -------------------------------------------------------------------------------- /Data/RGBD_K/feature_post_list.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/Data/RGBD_K/feature_post_list.mat -------------------------------------------------------------------------------- /Example_Junwang.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | addpath('./Data/RGBD_K'); 8 | 9 | [ Graph ] = InitializeGraph; 10 | 11 | R1= [ 0.860109 -0.014255 -0.509910 ; -0.000869 -0.999649 0.026480 ;-0.510109 -0.022333 -0.859820 ]; 12 | p1= [1.069093;1.390157;1.895715]; 13 | pose1=[R1 p1]; 14 | 15 | R2=[ 0.853790 -0.007297 -0.520566 ; 0.017675 -0.998919 0.042993 ;-0.520317 -0.045908 -0.852738 ]; 16 | p2= [1.095943;1.374461; 1.921930]; 17 | pose2=[R2 p2]; 18 | 19 | 20 | R3= [0.848725 -0.009615 -0.528747; 0.017975 -0.998732 0.047014 ;-0.528529 -0.049406 -0.847476 ]; 21 | p3= [1.112855;1.361110; 1.943661]; 22 | pose3=[R3 p3]; 23 | 24 | 25 | load feature_post_list.mat; 26 | 27 | M_initial.value = pose1; 28 | M_initial.inf=eye(6); 29 | [ Graph ] = AddUnaryEdge( Graph, 'PriorPose3_Factor', 'Pose3', 'pose1', M_initial ); 30 | 31 | 32 | 33 | Num_obs= size(feature_pose_list,1); 34 | for i=1:Num_obs 35 | 36 | landmark_id_i = ['landmark' num2str(feature_pose_list{i , 1})]; 37 | pose_id_i = ['pose' num2str(feature_pose_list{i , 3})]; 38 | uvd = feature_pose_list{i , 2}; 39 | localxyz = UVD2LocalXYZ( uvd ); 40 | Measurement.value= localxyz; 41 | Measurement.inf=eye(3); 42 | 43 | if isfield(Graph.Nodes, 'Landmark3') 44 | if isfield(Graph.Nodes.Landmark3.Values, landmark_id_i) 45 | NeedToSet=0; 46 | else NeedToSet=1; 47 | end 48 | else NeedToSet=1; 49 | end 50 | 51 | % [ Graph ]= AddNormalEdge(Graph, 'RGBD_Factor', 'Pose3', pose_id_i, 'Landmark3', landmark_id_i, Measurement); 52 | NodeArray=cell(2,2); 53 | NodeArray{1,1}='Pose3'; NodeArray{1,2}=pose_id_i; 54 | NodeArray{2,1}='Landmark3'; NodeArray{2,2}=landmark_id_i; 55 | [ Graph ]= AddComplexEdge(Graph, 'RGBD_Factor', NodeArray, Measurement); 56 | 57 | 58 | if NeedToSet==1 59 | fromPose=Graph.Nodes.Pose3.Values.(pose_id_i); 60 | switch pose_id_i 61 | case 'pose1' 62 | R=pose1(1:3,1:3); p = pose1(1:3,4); 63 | case 'pose2' 64 | R=pose2(1:3,1:3); p = pose2(1:3,4); 65 | case 'pose3' 66 | R=pose3(1:3,1:3); p = pose3(1:3,4); 67 | end 68 | Graph.Nodes.Landmark3.Values.(landmark_id_i )= R* localxyz +p; 69 | end 70 | end 71 | 72 | Graph.Nodes.Pose3.Values.pose1 = pose1; 73 | Graph.Nodes.Pose3.Values.pose2 = pose2; 74 | Graph.Nodes.Pose3.Values.pose3 = pose3; 75 | 76 | [ Graph ] = PerformGO( Graph ); 77 | 78 | feature_num = size( fieldnames(Graph.Nodes.Landmark3.Values), 1); 79 | feature_names = fieldnames(Graph.Nodes.Landmark3.Values); 80 | 81 | XX = zeros( 3, feature_num); 82 | for i=1:feature_num 83 | 84 | XX(:,i)= Graph.Nodes.Landmark3.Values.(feature_names{i}); 85 | 86 | end 87 | 88 | plot3( XX(1,:), XX(2,:), XX(3,:), 'o' ) 89 | axis equal 90 | 91 | 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /Example_VINS.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | addpath('./VINS_DataGenerator/'); 8 | 9 | load VINS_Cubic.mat 10 | 11 | [ Graph ] = InitializeGraph; 12 | 13 | 14 | %%% add Prior 15 | M_initial.value = RobotState{1, 1}.pose; 16 | M_initial.inf=100*eye(6); 17 | [ Graph ] = AddUnaryEdge( Graph, 'PriorPose3_Factor', 'Pose3', 'pose0', M_initial ); 18 | Graph.Nodes.Pose3.Values.pose0 = RobotState{1, 1}.pose; 19 | 20 | Measurement.value.vel = RobotState{1, 1}.velocity ; 21 | Measurement.value.bias = [RobotState{1, 1}.ba; RobotState{1, 1}.bw]; 22 | Measurement.inf=100*eye(9); 23 | NodeArray=cell(2,2); 24 | NodeArray{1,1}='Velocity3'; NodeArray{1,2}='v0'; 25 | NodeArray{2,1}='IMUbias'; NodeArray{2,2}='b0'; 26 | [ Graph ]= AddComplexEdge(Graph, 'PriorVelAndbias_Factor', NodeArray, Measurement); 27 | Graph.Nodes.Velocity3.Values.v0 = RobotState{1, 1}.velocity; 28 | Graph.Nodes.IMUbias.Values.b0 = Measurement.value.bias; 29 | 30 | 31 | %%% add IMU measurement 32 | num_poses = 20; 33 | 34 | for i = 0: num_poses-1 35 | %%%%%%%% IMU part 36 | NodeArray{1,1}='Pose3'; NodeArray{1,2}=['pose' num2str(i)]; 37 | NodeArray{2,1}='Velocity3'; NodeArray{2,2}=['v' num2str(i)]; 38 | NodeArray{3,1}='IMUbias'; NodeArray{3,2}=['b' num2str(i)]; 39 | NodeArray{4,1}='Pose3'; NodeArray{4,2}=['pose' num2str(i+1)]; 40 | NodeArray{5,1}='Velocity3'; NodeArray{5,2}=['v' num2str(i+1)]; 41 | NodeArray{6,1}='IMUbias'; NodeArray{6,2}=['b' num2str(i+1)]; 42 | Measurement_IMU_i = Measurement_IMU{i+1}; 43 | 44 | [ Graph ]= AddComplexEdge(Graph, 'IMU_Factor', NodeArray, Measurement_IMU_i); 45 | 46 | NodeArray_bias{1,1}='IMUbias'; NodeArray_bias{1,2}=['b' num2str(i)]; 47 | NodeArray_bias{2,1}='IMUbias'; NodeArray_bias{2,2}=['b' num2str(i+1)]; 48 | Measurement_IMU_change.value = []; 49 | Measurement_IMU_change.inf = IMUbiasWalk_inf; 50 | [ Graph ]= AddComplexEdge(Graph, 'IMUbias_Factor', NodeArray_bias, Measurement_IMU_change); 51 | 52 | Graph.Nodes.Pose3.Values.(NodeArray{1,2})=RobotState{i+1}.pose; 53 | Graph.Nodes.Pose3.Values.(NodeArray{4,2})=RobotState{i+2}.pose; 54 | 55 | Graph.Nodes.Velocity3.Values.(NodeArray{2,2})= RobotState{i+1}.velocity; 56 | Graph.Nodes.Velocity3.Values.(NodeArray{5,2})= RobotState{i+2}.velocity; 57 | Graph.Nodes.IMUbias.Values.(NodeArray{3,2})= [RobotState{i+1}.ba;RobotState{i+1}.bw]; 58 | Graph.Nodes.IMUbias.Values.(NodeArray{6,2})= [RobotState{i+2}.ba;RobotState{i+2}.bw]; 59 | 60 | %[ Graph ] = PerformGO( Graph ); % To get odometry 61 | 62 | end 63 | [ Graph ] = PerformGO( Graph ); 64 | 65 | 66 | 67 | % %%% add Vision measurement 68 | i=1; 69 | 70 | while( RGBD_data(i,1)<= num_poses) 71 | NodeArray=[]; 72 | pose_id = ['pose' num2str( RGBD_data(i,1) )]; 73 | landmark_id =['landmark' num2str( RGBD_data(i,2) )]; 74 | Measurement.value= (RGBD_data(i,3:5))' ; 75 | Measurement.inf=100*eye(3); 76 | 77 | NodeArray{1,1}='Pose3'; NodeArray{1,2}=pose_id; 78 | NodeArray{2,1}='Landmark3'; NodeArray{2,2}=landmark_id; 79 | 80 | [ Graph ]= AddComplexEdge(Graph, 'RGBD_Factor', NodeArray, Measurement); 81 | 82 | Graph.Nodes.Landmark3.Values.(landmark_id )= Landmarks.(landmark_id); 83 | 84 | i=i+1; 85 | end 86 | 87 | [ Graph ] = PerformGO_DL( Graph ); 88 | 89 | % T_truth = RobotState{1, 49 }.pose; R1 = T_truth(1:3,1:3); p1 = T_truth(1:3,4); 90 | % T_es = Graph.Nodes.Pose3.Values.pose48; R2 = T_es(1:3,1:3); p2 = T_es(1:3,4); 91 | % 92 | % dR = R1'*R2; 93 | % dp = R1'*(p2-p1); 94 | % dX =[dR dp]; 95 | % norm(se3_log( dX )) 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /Example_VINS_visual.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | addpath('./Geometry/'); 8 | addpath('./VINS_DataGenerator/'); 9 | 10 | load VINS_cubic3.mat 11 | 12 | [ Graph ] = InitializeGraph; 13 | 14 | 15 | %%% add Prior 16 | M_initial.value = RobotState{1, 1}.pose; 17 | M_initial.inf=100*eye(6); 18 | [ Graph ] = AddUnaryEdge( Graph, 'PriorPose3_Factor', 'Pose3', 'pose0', M_initial ); 19 | Graph.Nodes.Pose3.Values.pose0 = RobotState{1, 1}.pose; 20 | 21 | Measurement.value.vel = RobotState{1, 1}.velocity ; 22 | Measurement.value.bias = [RobotState{1, 1}.ba; RobotState{1, 1}.bw]; 23 | Measurement.inf=100*eye(9); 24 | NodeArray=cell(2,2); 25 | NodeArray{1,1}='Velocity3'; NodeArray{1,2}='v0'; 26 | NodeArray{2,1}='IMUbias'; NodeArray{2,2}='b0'; 27 | %[ Graph ]= AddComplexEdge(Graph, 'PriorVelAndbias_Factor', NodeArray, Measurement); 28 | % Graph.Nodes.Velocity3.Values.v0 = RobotState{1, 1}.velocity; 29 | % Graph.Nodes.IMUbias.Values.b0 = Measurement.value.bias; 30 | 31 | 32 | % 33 | Measure_translation.inf=eye(6)*1e5; 34 | T0= RobotState{1, 1}.pose; R0=T0(1:3,1:3); p0=T0(1:3,4); 35 | T1= RobotState{1, 2}.pose; R1=T1(1:3,1:3); p1=T1(1:3,4); 36 | 37 | 38 | Measure_translation.value=[R0'*R1 R0'*(p1-p0) ]; 39 | 40 | %[ Graph ] = AddNormalEdge( Graph, 'RelativePose3_Factor', 'Pose3', 'pose0', 'Pose3', 'pose1', Measure_translation ); 41 | 42 | 43 | 44 | 45 | 46 | %%% add IMU measurement 47 | num_poses = 49; 48 | 49 | for i = 0: num_poses-1 50 | %%%%%%%% IMU part 51 | NodeArray=cell(2,6); 52 | NodeArray{1,1}='Pose3'; NodeArray{1,2}=['pose' num2str(i)]; 53 | NodeArray{2,1}='Velocity3'; NodeArray{2,2}=['v' num2str(i)]; 54 | NodeArray{3,1}='IMUbias'; NodeArray{3,2}=['b' num2str(i)]; 55 | NodeArray{4,1}='Pose3'; NodeArray{4,2}=['pose' num2str(i+1)]; 56 | NodeArray{5,1}='Velocity3'; NodeArray{5,2}=['v' num2str(i+1)]; 57 | NodeArray{6,1}='IMUbias'; NodeArray{6,2}=['b' num2str(i+1)]; 58 | Measurement_IMU_i = Measurement_IMU{i+1}; 59 | 60 | [ Graph ]= AddComplexEdge(Graph, 'IMU_Factor', NodeArray, Measurement_IMU_i); 61 | 62 | NodeArray_bias{1,1}='IMUbias'; NodeArray_bias{1,2}=['b' num2str(i)]; 63 | NodeArray_bias{2,1}='IMUbias'; NodeArray_bias{2,2}=['b' num2str(i+1)]; 64 | Measurement_IMU_change.value = []; 65 | Measurement_IMU_change.inf = IMUbiasWalk_inf; 66 | [ Graph ]= AddComplexEdge(Graph, 'IMUbias_Factor', NodeArray_bias, Measurement_IMU_change); 67 | 68 | Graph.Nodes.Pose3.Values.(NodeArray{1,2})=RobotState{i+1}.pose; 69 | Graph.Nodes.Pose3.Values.(NodeArray{4,2})=RobotState{i+2}.pose; 70 | Graph.Nodes.Velocity3.Values.(NodeArray{2,2})= RobotState{i+1}.velocity; 71 | Graph.Nodes.Velocity3.Values.(NodeArray{5,2})= RobotState{i+2}.velocity; 72 | 73 | end 74 | [ Graph ] = PerformGO_DL( Graph ); 75 | 76 | 77 | 78 | % %%% add Vision measurement 79 | i=1; 80 | 81 | while( Measurement_Vision(i,1)<= num_poses) 82 | NodeArray=[]; 83 | pose_id = ['pose' num2str( Measurement_Vision(i,1) )]; 84 | pose_index = Measurement_Vision(i,1); 85 | landmark_id =['landmark' num2str( Measurement_Vision(i,2) )]; 86 | Measurement.value= (Measurement_Vision(i,3:4))' ; 87 | depth_rough = norm(RGBD_data(i,3:5))*(1+randn*0); 88 | %Measurement.inf= (1/depth_rough)^2*eye(2)*(525^2); 89 | Measurement.inf=eye(3)* (180/pi)^2*10; 90 | 91 | 92 | NodeArray{1,1}='Pose3'; NodeArray{1,2}=pose_id; 93 | NodeArray{2,1}='Landmark3'; NodeArray{2,2}=landmark_id; 94 | 95 | T= RobotState{pose_index+1}.pose; 96 | f = Landmarks.(landmark_id); 97 | R=T(1:3,1:3); t = T(1:3,4); 98 | z=R'*(f-t); 99 | 100 | if isfield(Graph.Nodes, 'Landmark3' ) 101 | if isfield(Graph.Nodes.Landmark3.Values, landmark_id) 102 | kk =1 ; 103 | else kk=0; 104 | end 105 | else kk=0; 106 | end 107 | 108 | if 1 109 | % Measurement.inf=eye(2); 110 | [ Graph ]= AddComplexEdge(Graph, 'VisionTest_Factor', NodeArray, Measurement); 111 | if kk == 1 112 | Graph.Nodes.Landmark3.Values.(landmark_id )= Landmarks.(landmark_id)+2*randn(3,1); 113 | end 114 | 115 | %pose = RobotState{pose_index+1}.pose; 116 | %R = pose(1:3,1:3)*expm(skew( randn(3,1)*0.3)); 117 | %t = pose(1:3,4)+randn(3,1)*0.6; 118 | 119 | %Graph.Nodes.Pose3.Values.(pose_id)=[ R t ]; 120 | end 121 | 122 | 123 | i=i+1; 124 | end 125 | 126 | S_max= find(Measurement_Vision(:,1)== num_poses , 1, 'last' ); 127 | 128 | [ LandmarkManager ] = Fn_LandmarkManager( Measurement_Vision(1: S_max, :) ); 129 | [ Landmarks_values ] = Cal_triangulate( Graph.Nodes.Pose3.Values , LandmarkManager ); 130 | Graph.Nodes.Landmark3.Values=Landmarks_values; 131 | 132 | subplot(3,1,1); 133 | PlotTrajectory( Graph ); 134 | title('Odometry And Triangulate'); 135 | 136 | % load gns_Linear.mat; 137 | % Graph.Nodes = GraphNodes; 138 | tic 139 | [ Graph ] = PerformGO_LM( Graph ); 140 | toc 141 | axis([-5 5 -5 5 -5 5]*8/5) 142 | subplot(3,1,2) 143 | PlotTrajectory( Graph ); 144 | title('Optimization'); 145 | axis([-5 5 -5 5 -5 5]*8/5) 146 | 147 | subplot(3,1,3) 148 | PlotTrajectory_Ground( RobotState, num_poses ); 149 | axis([-5 5 -5 5 -5 5]*8/5) 150 | 151 | 152 | 153 | 154 | -------------------------------------------------------------------------------- /Example_Vic_incre.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./Math/'); 5 | addpath('./Factor/'); 6 | addpath('./Data/2D_RGBD_Victoria_Park'); 7 | 8 | load Zstate_VicPark_6898_loops.mat; 9 | load CovMatrixInv_VicPark_6898_loops.mat; 10 | 11 | [ Graph ] = InitializeGraph; 12 | Graph.Parameters.iter= 50; 13 | Num_edge=15000; 14 | i=1; 15 | j=1; 16 | 17 | Total= size(Zstate, 1); 18 | 19 | M_initial.value=[eye(2) [0;0]]; 20 | M_initial.inf=eye(3)*100; 21 | [ Graph ] = AddUnaryEdge( Graph, 'PriorPose2_Factor', 'Pose2', 'pose0', M_initial ); 22 | 23 | odom_index=0; 24 | Measurement=cell(1,10000); 25 | %while (j<=Num_edge) 26 | while (i<=Total) 27 | 28 | 29 | if norm(Zstate(i,2)-2)< 0.001 30 | Factor_ThisStep='RGBD_2D_Factor'; 31 | pose_id = ['pose' num2str( Zstate(i,4) )]; 32 | landmark_id=['landmark' num2str(Zstate(i,3))]; 33 | Measurement{j}.value=[Zstate(i,1); Zstate(i+1,1)]; 34 | Measurement{j}.inf= full(CovMatrixInv( i:i+1, i:i+1) ); 35 | % Measurement{j}.inf= eye(2); 36 | 37 | if isfield(Graph.Nodes, 'Landmark2') 38 | if isfield(Graph.Nodes.Landmark2.Values, landmark_id) 39 | NeedToSet=0; 40 | else NeedToSet=1; 41 | end 42 | else NeedToSet=1; 43 | end 44 | 45 | 46 | 47 | 48 | 49 | [ Graph ] = AddNormalEdge( Graph, Factor_ThisStep, 'Pose2', pose_id, 'Landmark2', landmark_id, Measurement{j} ); 50 | 51 | 52 | if NeedToSet==1; 53 | fromPose=Graph.Nodes.Pose2.Values.(pose_id); 54 | R=fromPose(1:2,1:2); 55 | p=fromPose(1:2,3); 56 | Graph.Nodes.Landmark2.Values.(landmark_id )= R* [Zstate(i,1); Zstate(i+1,1)] +p; 57 | end 58 | 59 | i=i+2; 60 | else 61 | Factor_ThisStep='RelativePose2_Factor'; 62 | from_pose_id =['pose' num2str( Zstate(i,4) )]; 63 | to_pose_id=['pose' num2str(Zstate(i,4)+1 )]; 64 | Measurement{j}.value=[Zstate(i,1); Zstate(i+1,1); Zstate(i+2,1) ]; 65 | Measurement{j}.inf= full(CovMatrixInv( i:i+2, i:i+2 )); 66 | % Measurement{j}.inf= eye(3); 67 | [ Graph ] = AddNormalEdge( Graph, Factor_ThisStep, 'Pose2', from_pose_id, 'Pose2', to_pose_id, Measurement{j} ); 68 | 69 | fromPose=Graph.Nodes.Pose2.Values.(from_pose_id); 70 | theta=Zstate(i+2,1); 71 | v=[Zstate(i,1); Zstate(i+1,1)]; 72 | R=fromPose(1:2,1:2); 73 | p=fromPose(1:2,3); 74 | dR= [cos(theta ) -sin(theta); sin(theta) cos(theta)]; 75 | Graph.Nodes.Pose2.Values.(to_pose_id)=[R*dR R*v+p]; 76 | 77 | 78 | % if abs(mod(odom_index,200)-0)<0.0001 79 | % [ Graph ] = PerformGO( Graph ); % incremental optimization 80 | % end 81 | 82 | odom_index=odom_index+1; 83 | 84 | i=i+3 85 | end 86 | 87 | %[ Graph ] = PerformGO( Graph ); 88 | j=j+1; 89 | end 90 | 91 | % give an ancor 92 | 93 | %%%% optimization 94 | [ Graph ] = PerformGO( Graph ); 95 | %%% plot 96 | PoseArray = fields(Graph.Nodes.Pose2.Values); 97 | Num_poses= size(PoseArray,1); 98 | XX=[]; 99 | YY=[]; 100 | for i=1:Num_poses 101 | 102 | ThisPose_id= PoseArray{i}; 103 | 104 | ThisPose_value=Graph.Nodes.Pose2.Values.(ThisPose_id); 105 | 106 | ThisPose_position=ThisPose_value(1:2,3); 107 | 108 | x=ThisPose_position(1); 109 | y=ThisPose_position(2); 110 | 111 | XX=[XX x]; 112 | YY=[YY y]; 113 | 114 | end 115 | 116 | %plot(XX,YY,'LineWidth',3); 117 | plot(XX,YY,'-'); 118 | axis equal 119 | axis([-150 250 -100 300]) -------------------------------------------------------------------------------- /Example_VictoriaPark.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./Math/'); 5 | addpath('./Factor/'); 6 | addpath('./Data/2D_RGBD_Victoria_Park'); 7 | 8 | load Zstate_VicPark_6898_loops.mat; 9 | load CovMatrixInv_VicPark_6898_loops.mat; 10 | 11 | [ Graph ] = InitializeGraph; 12 | 13 | Num_edge=2250; 14 | i=1; 15 | j=1; 16 | 17 | Total= size(Zstate, 1); 18 | 19 | % give an ancor 20 | MeasurementS.value=[eye(2) [0;0]]; 21 | MeasurementS.inf=eye(3)*100; 22 | [ Graph ] = AddUnaryEdge( Graph, 'PriorPose2_Factor', 'Pose2', 'pose0', MeasurementS ); 23 | 24 | while (j<=Num_edge) 25 | %while (i<=Total) 26 | 27 | 28 | 29 | if norm(Zstate(i,2)-2)< 0.001 30 | Factor_ThisStep='RGBD_2D_Factor'; 31 | pose_id = ['pose' num2str( Zstate(i,4) )]; 32 | landmark_id=['landmark' num2str(Zstate(i,3))]; 33 | Measurement.value=[Zstate(i,1); Zstate(i+1,1)]; 34 | %Measurement{j}.inf= full(CovMatrixInv( i:i+1, i:i+1) ); 35 | Measurement.inf=eye(2); 36 | 37 | [ Graph ] = AddNormalEdge( Graph, Factor_ThisStep, 'Pose2', pose_id, 'Landmark2', landmark_id, Measurement ); 38 | 39 | i=i+2; 40 | else 41 | Factor_ThisStep='RelativePose2_Factor'; 42 | from_pose_id =['pose' num2str( Zstate(i,4) )]; 43 | to_pose_id=['pose' num2str(Zstate(i,3) )]; 44 | 45 | Measurement.value=[Zstate(i,1); Zstate(i+1,1); Zstate(i+2,1) ]; 46 | %Measurement{j}.inf= full(CovMatrixInv( i:i+2, i:i+2 )); 47 | Measurement.inf=eye(3); 48 | 49 | [ Graph ] = AddNormalEdge( Graph, Factor_ThisStep, 'Pose2', from_pose_id, 'Pose2', to_pose_id, Measurement ); 50 | i=i+3 51 | end 52 | 53 | 54 | j=j+1; 55 | end 56 | 57 | 58 | 59 | 60 | %%%% optimization 61 | [ Graph ] = PerformGO( Graph ); 62 | %%% plot 63 | PoseArray = fields(Graph.Nodes.Pose2.Values); 64 | Num_poses= size(PoseArray,1); 65 | XX=[]; 66 | YY=[]; 67 | for i=1:Num_poses 68 | 69 | ThisPose_id= PoseArray{i}; 70 | 71 | ThisPose_value=Graph.Nodes.Pose2.Values.(ThisPose_id); 72 | 73 | ThisPose_position=ThisPose_value(1:2,3); 74 | 75 | x=ThisPose_position(1); 76 | y=ThisPose_position(2); 77 | 78 | XX=[XX x]; 79 | YY=[YY y]; 80 | 81 | end 82 | 83 | plot(XX,YY,'LineWidth',3); 84 | axis equal 85 | axis([-150 250 -100 300]) 86 | -------------------------------------------------------------------------------- /Factor/GetEdgeTypeDimension.m: -------------------------------------------------------------------------------- 1 | function [ dimension ] = GetEdgeTypeDimension( EdgeTypeName ) 2 | 3 | 4 | dimension=0; 5 | 6 | switch EdgeTypeName 7 | case 'PriorPose3_Factor' 8 | dimension=6; 9 | case 'PriorPose2_Factor' 10 | dimension=3; 11 | case 'RelativePose3_Factor' 12 | dimension=6; 13 | case 'RelativePose2_Factor' 14 | dimension=3; 15 | case 'LinearVision_Factor' 16 | dimension=2; 17 | case 'IMU_Factor' 18 | dimension=9; 19 | case 'RGBD_Factor' 20 | dimension=3; 21 | case 'RGBD_2D_Factor' 22 | dimension=2; 23 | case 'Vision_Factor' 24 | dimension=2; 25 | case 'VisionPoseFixed_Factor' 26 | dimension=2; 27 | case 'VisionTest_Factor' 28 | dimension=3; 29 | case 'VisionTestPoseFixed_Factor' 30 | dimension=3; 31 | case 'IMUbias_Factor' 32 | dimension=6; 33 | case 'PriorVelAndbias_Factor' 34 | dimension=9; 35 | case 'VisionNoLandmark_Factor' 36 | dimension=2; 37 | case 'LinearVisionNoLandmark_Factor' 38 | dimension=2; 39 | case 'VisionPBA_Factor' 40 | dimension=2; 41 | case 'VisionPBA_MainAnchor_Factor' 42 | dimension=2; 43 | case 'VisionPBA_AssAnchor_Factor' 44 | dimension=2; 45 | case 'VisionTestPBA_Factor' 46 | dimension=3; 47 | case 'VisionTestPBA_MainAnchor_Factor' 48 | dimension=3; 49 | case 'VisionTestPBA_AssAnchor_Factor' 50 | dimension=3; 51 | end 52 | 53 | 54 | if dimension==0 55 | 56 | sprintf('Warning: you have not defined the new edge dimension in GetEdgeTypeDimension.m') 57 | 58 | end 59 | 60 | 61 | 62 | end 63 | 64 | -------------------------------------------------------------------------------- /Factor/GetFactorX_format.m: -------------------------------------------------------------------------------- 1 | function [ Nodes_array ] = GetFactorX_format( EdgeTypeName ) 2 | flag=0; 3 | switch EdgeTypeName 4 | case 'PriorPose3_Factor' 5 | Nodes_array{1}=[]; flag=1; 6 | case 'PriorPose2_Factor' 7 | Nodes_array{1}=[]; flag=1; 8 | case 'RelativePose3_Factor' 9 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 10 | case 'RelativePose2_Factor' 11 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 12 | case 'Vision3_Factor' 13 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 14 | case 'IMU_Factor' 15 | Nodes_array=cell(6,1); flag=1; 16 | case 'IMUbias_Factor' 17 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 18 | case 'RGBD_Factor' 19 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 20 | case 'RGBD_2D_Factor' 21 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 22 | case 'Vision_Factor' 23 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 24 | case 'VisionTest_Factor' 25 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 26 | case 'VisionTestPoseFixed_Factor' 27 | Nodes_array{1}=[]; flag=1; 28 | case 'VisionPoseFixed_Factor' 29 | Nodes_array{1}=[]; flag=1; 30 | case 'PriorVelAndbias_Factor' 31 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 32 | case 'VisionNoLandmark_Factor' 33 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 34 | case 'LinearVisionNoLandmark_Factor' 35 | Nodes_array{1}=[]; Nodes_array{2}=[]; flag=1; 36 | case 'VisionPBA_Factor' 37 | Nodes_array=cell(4,1) ; flag=1; 38 | case 'VisionPBA_MainAnchor_Factor' 39 | Nodes_array=cell(1,1) ; flag=1; 40 | case 'VisionPBA_AssAnchor_Factor' 41 | Nodes_array=cell(3,1) ; flag=1; 42 | case 'VisionTestPBA_Factor' 43 | Nodes_array=cell(4,1) ; flag=1; 44 | case 'VisionTestPBA_MainAnchor_Factor' 45 | Nodes_array=cell(1,1) ; flag=1; 46 | case 'VisionTestPBA_AssAnchor_Factor' 47 | Nodes_array=cell(3,1) ; flag=1; 48 | end 49 | 50 | if flag==0 51 | 52 | sprintf('Warning: you have not defined the new edge format in GetFactorX_format.m') 53 | 54 | end 55 | 56 | 57 | end 58 | 59 | -------------------------------------------------------------------------------- /Factor/GetNodeTypeDimension.m: -------------------------------------------------------------------------------- 1 | function [ dimension ] = GetNodeTypeDimension( NodeTypeName ) 2 | 3 | dimension=0; 4 | 5 | switch NodeTypeName 6 | case 'Pose3' 7 | dimension=6; 8 | case 'LPose3' 9 | dimension=6; 10 | case 'Pose2' 11 | dimension=3; 12 | case 'Landmark3' 13 | dimension=3; 14 | case 'Landmark2' 15 | dimension=2; 16 | case 'Velocity3' 17 | dimension=3; 18 | case 'IMUbias' 19 | dimension=6; 20 | case 'ParallaxPoint' 21 | dimension=3; 22 | end 23 | 24 | 25 | if dimension==0 26 | 27 | sprintf('Warning: you have to define the new Node dimension in GetNodeTypeDimension.m') 28 | 29 | end 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /Factor/IMU_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = IMU_Factor( Nodes_array , Measurement_values ) 2 | g=[0;0;-9.8]; 3 | dt = Measurement_values.dt; 4 | 5 | posei=Nodes_array{1}; vi=Nodes_array{2}; bi=Nodes_array{3}; 6 | Ri=posei(1:3,1:3); pi=posei(1:3,4); 7 | posej=Nodes_array{4}; vj=Nodes_array{5}; bj=Nodes_array{6}; 8 | Rj=posej(1:3,1:3); pj=posej(1:3,4); 9 | dbw=bi(4:6)-Measurement_values.bw; 10 | dba=bi(1:3)-Measurement_values.ba; 11 | 12 | %dbw=zeros(3,1); 13 | %dba=zeros(3,1); 14 | 15 | [ Rj_pre, vj_pre, pj_pre ]=Prediction_IMU(posei, vi, bi, Measurement_values ); 16 | Xj_pre_inv = [ Rj_pre' -vj_pre -Rj_pre'*pj_pre ]; 17 | dR= Rj'*Rj_pre; 18 | dX = [ dR dR*vj_pre-vj Rj'*(pj_pre - pj)]; 19 | 20 | ErrorVector_robot = se23_log( dX ); 21 | ErrorVector= [ ErrorVector_robot ]; 22 | 23 | %%%%% Notation 24 | J1 = Measurement_values.J1; 25 | j1_w = Measurement_values.j1_w; 26 | J2 = Measurement_values.J2; 27 | j2_w = Measurement_values.j2_w; 28 | j2_a = Measurement_values.j2_a; 29 | J3 = Measurement_values.J3; 30 | j3_w = Measurement_values.j3_w; 31 | j3_a = Measurement_values.j3_a; 32 | %%%%% Notation 33 | 34 | 35 | A = Jacobian_Lie_inverse(ErrorVector_robot)*adjoint(Xj_pre_inv); 36 | 37 | %A = adjoint(Xj_pre_inv); 38 | 39 | zero33=zeros(3,3); 40 | 41 | % M = [eye(3) zero33 zero33 zero33 Ri*J1*Jacobian_Lie(-j1_w*dbw );... 42 | % skew(g)*dt Ri zero33 Ri*j2_a Ri*j2_w; ... 43 | % skew(g)*dt^2/2 Ri*dt eye(3) Ri*j3_a Ri*j3_w ]; 44 | Vi = Ri*vi; 45 | 46 | M = [eye(3) zero33 zero33 zero33 Ri*J1*j1_w;... 47 | skew(g)*dt Ri zero33 Ri*j2_a Ri*j2_w+skew(Vi+g*dt+Ri*J2)*Ri*J1*j1_w; ... 48 | skew(g)*dt^2/2 Ri*dt eye(3) Ri*j3_a Ri*j3_w+skew(pi+1/2*g*dt^2+Vi*dt+Ri*J3)*Ri*J1*j1_w ]; 49 | 50 | 51 | 52 | N = [ eye(3) zeros(3,12); zero33 Rj zeros(3,9); zeros(3,6) eye(3) zeros(3,6) ]; 53 | 54 | 55 | Jacobian_whole = A*[M -N]; 56 | 57 | Jacobian_Node{1} = [ Jacobian_whole(:, 1:3) Jacobian_whole(:, 7:9)]; 58 | Jacobian_Node{2} = [ Jacobian_whole(:, 4:6) ]; 59 | Jacobian_Node{3} = [ Jacobian_whole(:, 10:15)]; 60 | 61 | Jacobian_Node{4} = [Jacobian_whole(:, 16: 18 ) Jacobian_whole(:, 22:24) ]; 62 | Jacobian_Node{5} = [ Jacobian_whole(:, 19 : 21)]; 63 | Jacobian_Node{6} = [ Jacobian_whole(:, 25:30) ]; 64 | 65 | 66 | 67 | 68 | end -------------------------------------------------------------------------------- /Factor/IMUbias_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = IMUbias_Factor( Nodes_array , Measurement_values ) 2 | 3 | bi=Nodes_array{1}; 4 | bj=Nodes_array{2}; 5 | 6 | 7 | ErrorVector=bi-bj; 8 | 9 | Jacobian_Node{1}=eye(6); 10 | Jacobian_Node{2}=-eye(6); 11 | 12 | end 13 | 14 | -------------------------------------------------------------------------------- /Factor/LinearVisionNoLandmark_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = LinearVisionNoLandmark_Factor( Nodes_array , Measurement_values ) 2 | 3 | pose=Nodes_array{1}; 4 | 5 | f=Measurement_values.f; 6 | uv=Measurement_values.uv; 7 | R=pose(1:3,1:3); 8 | p=pose(1:3,4); 9 | Local= R'*( f - p ); 10 | 11 | 12 | fx = 525.0; 13 | fy = 525.0; 14 | cx0 = 639.5; 15 | cy0 = 479.5; 16 | 17 | x= Local(1); 18 | y= Local(2); 19 | z= Local(3); 20 | 21 | K = [ fx 0; 0 fy]; 22 | 23 | 24 | 25 | K_inv = [ 1/fx 0; 0 1/fy]; 26 | 27 | A = K_inv*(uv - [cx0;cy0] ); 28 | AA = [ eye(2) -A]; 29 | 30 | ErrorVector= AA*Local; 31 | 32 | 33 | 34 | Jacobian_Node{1}= AA*[ R'*skew(f) -R']; 35 | 36 | 37 | 38 | 39 | end 40 | 41 | -------------------------------------------------------------------------------- /Factor/LinearVision_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = LinearVision_Factor( Nodes_array , Measurement_values ) 2 | 3 | pose=Nodes_array{1}; 4 | f=Nodes_array{2}; 5 | R=pose(1:3,1:3); 6 | p=pose(1:3,4); 7 | Local= R'*( f - p ); 8 | 9 | 10 | 11 | 12 | fx = 525.0; 13 | fy = 525.0; 14 | cx0 = 639.5; 15 | cy0 = 479.5; 16 | 17 | 18 | 19 | K_inv = [ 1/fx 0; 0 1/fy]; 20 | 21 | A = K_inv*(Measurement_values - [cx0;cy0] ); 22 | AA = [ eye(2) -A]; 23 | 24 | ErrorVector= AA*Local; 25 | 26 | 27 | Jacobian_Node{1}=AA*[ R'*skew(f) -R']; 28 | Jacobian_Node{2}=AA*R'; 29 | 30 | 31 | 32 | end 33 | 34 | -------------------------------------------------------------------------------- /Factor/PBA_factor/DirectProject.m: -------------------------------------------------------------------------------- 1 | function [ y ] = DirectProject( x ) 2 | 3 | 4 | y = x/norm(x); 5 | 6 | 7 | 8 | end 9 | 10 | -------------------------------------------------------------------------------- /Factor/PBA_factor/NormalFromUV.m: -------------------------------------------------------------------------------- 1 | function [ nuv ] = NormalFromUV( uv ) 2 | fx = 525.0; 3 | fy = 525.0; 4 | cx0 = 639.5; 5 | cy0 = 479.5; 6 | n = diag([1/fx, 1/fy] ) *(uv - [cx0;cy0]); 7 | m = [n;1]; 8 | 9 | nuv = m/norm(m); 10 | 11 | 12 | end 13 | 14 | -------------------------------------------------------------------------------- /Factor/PBA_factor/VisionPBA_AssAnchor_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionPBA_AssAnchor_Factor( Nodes_array , Measurement_values ) 2 | 3 | pose_m=Nodes_array{1}; 4 | pose_a=Nodes_array{2}; 5 | feature=Nodes_array{3}; 6 | 7 | uv = Measurement_values.uv; 8 | Tcb= Measurement_values.Tcb; 9 | 10 | 11 | 12 | [X, J_m , J_a, J_i , J_f] = PBA_Jacobian( pose_m, pose_a, pose_a, feature, Tcb); 13 | 14 | ErrorVector= project(X)-uv; 15 | 16 | if norm(ErrorVector)>2 17 | 18 | sss= 0; 19 | 20 | end 21 | 22 | 23 | Jacobian_Node{1} = dproject(X)*J_m; 24 | Jacobian_Node{2} = dproject(X)*(J_a+J_i); 25 | Jacobian_Node{3} = dproject(X)*J_f; 26 | 27 | 28 | end 29 | 30 | function project_X=project(X) 31 | 32 | fx = 525.0; 33 | fy = 525.0; 34 | cx0 = 639.5; 35 | cy0 = 479.5; 36 | x=X(1); 37 | y=X(2); 38 | z=X(3); 39 | project_X = [ fx*x/z; fy*y/z]+[cx0;cy0]; 40 | 41 | end 42 | 43 | function dX=dproject(X) 44 | x=X(1); 45 | y=X(2); 46 | z=X(3); 47 | fx = 525.0; 48 | fy = 525.0; 49 | K = [ fx 0; 0 fy]; 50 | dX = K* [z 0 -x; 0 z -y ]/(z^2); 51 | end -------------------------------------------------------------------------------- /Factor/PBA_factor/VisionPBA_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionPBA_Factor( Nodes_array , Measurement_values ) 2 | 3 | pose_m=Nodes_array{1}; 4 | pose_a=Nodes_array{2}; 5 | pose=Nodes_array{3}; 6 | feature=Nodes_array{4}; 7 | 8 | uv = Measurement_values.uv; 9 | Tcb= Measurement_values.Tcb; 10 | 11 | 12 | 13 | [X, J_m , J_a, J_i , J_f] = PBA_Jacobian( pose_m, pose_a, pose, feature, Tcb); 14 | 15 | ErrorVector= project(X)-uv; 16 | 17 | if norm(ErrorVector)>2 18 | 19 | ss =0; 20 | end 21 | 22 | 23 | Jacobian_Node{1} = dproject(X)*J_m; 24 | Jacobian_Node{2} = dproject(X)*J_a; 25 | Jacobian_Node{3} = dproject(X)*J_i; 26 | Jacobian_Node{4} = dproject(X)*J_f; 27 | 28 | 29 | end 30 | 31 | function project_X=project(X) 32 | 33 | fx = 525.0; 34 | fy = 525.0; 35 | cx0 = 639.5; 36 | cy0 = 479.5; 37 | x=X(1); 38 | y=X(2); 39 | z=X(3); 40 | project_X = [ fx*x/z; fy*y/z]+[cx0;cy0]; 41 | 42 | end 43 | 44 | function dX=dproject(X) 45 | x=X(1); 46 | y=X(2); 47 | z=X(3); 48 | fx = 525.0; 49 | fy = 525.0; 50 | K = [ fx 0; 0 fy]; 51 | dX = K* [z 0 -x; 0 z -y ]/(z^2); 52 | end -------------------------------------------------------------------------------- /Factor/PBA_factor/VisionPBA_MainAnchor_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionPBA_MainAnchor_Factor( Nodes_array , Measurement_values ) 2 | 3 | %pose_m=Nodes_array{1}; 4 | %pose_a=Nodes_array{2}; 5 | feature=Nodes_array{1}; 6 | 7 | uv = Measurement_values.uv; 8 | %Tcb= Measurement_values.Tcb; 9 | 10 | 11 | 12 | %[X, J_m , J_a, J_i , J_f] = PBA_Jacobian( pose_m, pose_a, pose_m, feature, Tcb); 13 | 14 | n = feature(1:3,1); 15 | A = computeA(n); 16 | 17 | ErrorVector= project(n)-uv; 18 | 19 | 20 | 21 | 22 | %Jacobian_Node{1} = dproject(X)*(J_m+J_i); 23 | %Jacobian_Node{2} = dproject(X)*J_a; 24 | Jacobian_Node{1} = dproject(n)*[-skew(n)*A [0;0;0]]; 25 | 26 | 27 | end 28 | 29 | function project_X=project(X) 30 | 31 | fx = 525.0; 32 | fy = 525.0; 33 | cx0 = 639.5; 34 | cy0 = 479.5; 35 | x=X(1); 36 | y=X(2); 37 | z=X(3); 38 | project_X = [ fx*x/z; fy*y/z]+[cx0;cy0]; 39 | 40 | end 41 | 42 | function dX=dproject(X) 43 | x=X(1); 44 | y=X(2); 45 | z=X(3); 46 | fx = 525.0; 47 | fy = 525.0; 48 | K = [ fx 0; 0 fy]; 49 | dX = K* [z 0 -x; 0 z -y ]/(z^2); 50 | end -------------------------------------------------------------------------------- /Factor/PBA_factor/VisionTestPBA_AssAnchor_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionTestPBA_AssAnchor_Factor( Nodes_array , Measurement_values ) 2 | 3 | pose_m=Nodes_array{1}; 4 | pose_a=Nodes_array{2}; 5 | feature=Nodes_array{3}; 6 | 7 | uv = Measurement_values.uv; 8 | Tcb= Measurement_values.Tcb; 9 | 10 | 11 | 12 | [X, J_m , J_a, J_i , J_f] = PBA_Jacobian( pose_m, pose_a, pose_a, feature, Tcb); 13 | %[X, J_m , J_a, J_f] = PBA_JacobianAss( pose_m, pose_a, feature, Tcb); 14 | 15 | 16 | ErrorVector= DirectProject(X)-NormalFromUV(uv); 17 | 18 | if norm(ErrorVector)>2 19 | 20 | sss= 0; 21 | 22 | end 23 | 24 | 25 | Jacobian_Node{1} = dDirectProject(X)*J_m; 26 | Jacobian_Node{2} = dDirectProject(X)*(J_a+J_i); 27 | %Jacobian_Node{2} = dDirectProject(X)*J_a; 28 | Jacobian_Node{3} = dDirectProject(X)*J_f; 29 | 30 | 31 | end -------------------------------------------------------------------------------- /Factor/PBA_factor/VisionTestPBA_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionTestPBA_Factor( Nodes_array, Measurement_values ) 2 | 3 | pose_m=Nodes_array{1}; 4 | pose_a=Nodes_array{2}; 5 | pose=Nodes_array{3}; 6 | feature=Nodes_array{4}; 7 | 8 | uv = Measurement_values.uv; 9 | Tcb= Measurement_values.Tcb; 10 | 11 | 12 | 13 | [X, J_m , J_a, J_i , J_f] = PBA_Jacobian( pose_m, pose_a, pose, feature, Tcb); 14 | 15 | ErrorVector= DirectProject(X)-NormalFromUV(uv); 16 | 17 | if norm(ErrorVector)>2 18 | 19 | ss =0; 20 | end 21 | 22 | 23 | Jacobian_Node{1} = dDirectProject(X)*J_m; 24 | Jacobian_Node{2} = dDirectProject(X)*J_a; 25 | Jacobian_Node{3} = dDirectProject(X)*J_i; 26 | Jacobian_Node{4} = dDirectProject(X)*J_f; 27 | 28 | 29 | end -------------------------------------------------------------------------------- /Factor/PBA_factor/VisionTestPBA_MainAnchor_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionTestPBA_MainAnchor_Factor( Nodes_array , Measurement_values ) 2 | 3 | %pose_m=Nodes_array{1}; 4 | %pose_a=Nodes_array{2}; 5 | feature=Nodes_array{1}; 6 | 7 | uv = Measurement_values.uv; 8 | %Tcb= Measurement_values.Tcb; 9 | 10 | 11 | n = feature(1:3,1); 12 | A = computeA(n); 13 | 14 | 15 | ErrorVector= n-NormalFromUV(uv); 16 | 17 | 18 | 19 | %Jacobian_Node{1} = zeros(3,6); 20 | %Jacobian_Node{2} = zeros(3,6); 21 | Jacobian_Node{1} = [-skew(n)*A [0;0;0]]; 22 | 23 | 24 | 25 | % Jacobian_Node{1} = dDirectProject(X)*(J_m+J_i); 26 | % Jacobian_Node{2} = dDirectProject(X)*J_a; 27 | % Jacobian_Node{3} = dDirectProject(X)*J_f; 28 | 29 | 30 | end 31 | -------------------------------------------------------------------------------- /Factor/PBA_factor/dDirectProject.m: -------------------------------------------------------------------------------- 1 | function [ y ] = dDirectProject( x ) 2 | 3 | d = norm(x); 4 | 5 | y = 1/d*eye(3)- x*x'/d^3; 6 | 7 | end 8 | 9 | -------------------------------------------------------------------------------- /Factor/Prediction_IMU.m: -------------------------------------------------------------------------------- 1 | function [ Rj_pre, vj_pre, pj_pre ] = Prediction_IMU( posei, vi, bi, Measurement_values ) 2 | 3 | %%%%%%% Notation Definition 4 | 5 | dbw=bi(4:6)-Measurement_values.bw; 6 | dba=bi(1:3)-Measurement_values.ba; 7 | 8 | % dbw=[0;0;0]; 9 | % dba=[0;0;0]; 10 | 11 | 12 | J1 = Measurement_values.J1; 13 | j1_w = Measurement_values.j1_w; 14 | 15 | J2 = Measurement_values.J2; 16 | j2_w = Measurement_values.j2_w; 17 | j2_a = Measurement_values.j2_a; 18 | 19 | 20 | J3 = Measurement_values.J3; 21 | j3_w = Measurement_values.j3_w; 22 | j3_a = Measurement_values.j3_a; 23 | 24 | 25 | dt=Measurement_values.dt; 26 | g=[0;0;-9.8]; 27 | 28 | Ri = posei(1:3,1:3); 29 | pi = posei(1:3,4); 30 | 31 | 32 | %%%%%%%% Prediction 33 | 34 | Rj_pre= Ri*J1*so3_exp( j1_w * dbw ); 35 | 36 | Vj_pre= Ri*vi + dt*g+ Ri*( J2+ j2_w* dbw + j2_a*dba ); 37 | 38 | vj_pre= Rj_pre'* Vj_pre; 39 | 40 | pj_pre= pi + dt^2/2*g + dt*Ri*vi+Ri*(J3+ j3_w* dbw + j3_a*dba ); 41 | 42 | 43 | 44 | 45 | 46 | end 47 | 48 | -------------------------------------------------------------------------------- /Factor/PriorPose2_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = PriorPose2_Factor( Nodes_array , Measurement_values ) 2 | X=Nodes_array{1}; 3 | R_x=X(1:2,1:2); R_pr=Measurement_values(1:2,1:2); 4 | p_x=X(1:2,3); p_pr=Measurement_values(1:2,3); 5 | 6 | 7 | 8 | dR=R_x*R_pr'; 9 | dtheta=atan2( dR(2,1), dR(1,1) ); 10 | dP= p_x- p_pr; 11 | 12 | 13 | ErrorVector=[ dtheta; dP ]; 14 | 15 | Jacobian_Node{1} = eye(3); 16 | 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /Factor/PriorPose3_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = PriorPose3_Factor( Nodes_array , Measurement_values ) 2 | X=Nodes_array{1}; 3 | R_x=X(1:3,1:3); R_pr=Measurement_values(1:3,1:3); 4 | p_x=X(1:3,4); p_pr=Measurement_values(1:3,4); 5 | 6 | 7 | 8 | dR=R_x*R_pr'; 9 | dtheta=so3_log( dR ); 10 | dP= p_x- dR*p_pr; 11 | dp= jacor_inverse(-dtheta)*dP; 12 | 13 | 14 | ErrorVector=[ dtheta; dp ]; 15 | 16 | Jacobian_Node{1} = Jacobian_Lie_inverse ( - ErrorVector ); 17 | 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /Factor/PriorVelAndbias_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = PriorVelAndbias_Factor( Nodes_array , Measurement_values ) 2 | 3 | v = Nodes_array{1}; 4 | b = Nodes_array{2}; 5 | 6 | v_prior = Measurement_values.vel; 7 | b_prior = Measurement_values.bias; 8 | 9 | 10 | ErrorVector = [ v-v_prior; b- b_prior ]; 11 | 12 | Jacobian_Node{1} = [eye(3); zeros(6,3) ]; 13 | 14 | Jacobian_Node{2} = [zeros(3,6); eye(6) ]; 15 | 16 | 17 | end 18 | 19 | -------------------------------------------------------------------------------- /Factor/RGBD_2D_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = RGBD_2D_Factor( Nodes_array , Measurement_values ) 2 | 3 | pose=Nodes_array{1}; 4 | f=Nodes_array{2}; 5 | 6 | relative_measurement= Measurement_values; 7 | 8 | R=pose(1:2,1:2); 9 | p=pose(1:2,3); 10 | 11 | ErrorVector = R'*( f- p) - relative_measurement; 12 | 13 | J = [0 -1 ; 1 0 ]; 14 | 15 | Jacobian_Node{1}= [ -R'*J* ( f -p ) -R' ]; 16 | 17 | Jacobian_Node{2}= R'; 18 | 19 | 20 | 21 | end 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /Factor/RGBD_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = RGBD_Factor( Nodes_array , Measurement_values ) 2 | 3 | pose=Nodes_array{1}; 4 | f=Nodes_array{2}; 5 | 6 | relative_measurement= Measurement_values; 7 | 8 | R=pose(1:3,1:3); 9 | p=pose(1:3,4); 10 | 11 | ErrorVector = R'*( f- p) - relative_measurement; 12 | 13 | Jacobian_Node{1}= [ R'*skew( f ) -R' ]; 14 | 15 | Jacobian_Node{2}= R'; 16 | 17 | 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /Factor/RelativePose2_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = RelativePose2_Factor( Nodes_array , Measurement_values ) 2 | 3 | NodeA=Nodes_array{1}; 4 | NodeB=Nodes_array{2}; 5 | 6 | 7 | R_A=NodeA(1:2,1:2); p_A=NodeA(1:2,3); 8 | R_B=NodeB(1:2,1:2); p_B=NodeB(1:2,3); 9 | theta=Measurement_values(3); v=Measurement_values(1:2); 10 | R_u=[cos(theta) -sin(theta ); sin(theta) cos(theta) ]; 11 | 12 | dR= R_A'*R_B*R_u'; 13 | dP= R_A'*( p_B-p_A )-v; 14 | 15 | 16 | dtheta= atan2( dR(2,1), dR(1,1) ) ; 17 | 18 | ErrorVector=[ dP ; dtheta ]; % different from 3D 19 | 20 | J=[0 -1; 1 0]; 21 | Jacobian_NodeA = [ -J*R_A'*( p_B-p_A ) -R_A'; -1 zeros(1,2) ]; 22 | Jacobian_NodeB = [ zeros(2,1) R_A'; 1 zeros(1,2) ]; 23 | 24 | Jacobian_Node{1}=Jacobian_NodeA; 25 | Jacobian_Node{2}=Jacobian_NodeB; 26 | 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /Factor/RelativePose3_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = RelativePose3_Factor( Nodes_array , Measurement_values ) 2 | 3 | NodeA=Nodes_array{1}; 4 | NodeB=Nodes_array{2}; 5 | 6 | 7 | R_A=NodeA(1:3,1:3); p_A=NodeA(1:3,4); 8 | R_B=NodeB(1:3,1:3); p_B=NodeB(1:3,4); 9 | R_u=Measurement_values(1:3,1:3); p_u=Measurement_values(1:3,4); 10 | 11 | 12 | dR= R_A'*R_B*R_u'; 13 | dP= -R_A'*R_B*R_u'*p_u + R_A'*( p_B-p_A ); 14 | 15 | 16 | dtheta=so3_log( dR ); 17 | dp = jacor_inverse(-dtheta)*dP; 18 | 19 | ErrorVector=[ dtheta; dp ]; 20 | 21 | JrE=Jacobian_Lie_inverse(-ErrorVector); 22 | %JrE=eye(6); 23 | 24 | 25 | X_Ainv=[R_A' -R_A'*p_A ]; 26 | adX_Ainv= adjoint( X_Ainv ) ; 27 | 28 | Jacobian_NodeA=-JrE*adX_Ainv; 29 | Jacobian_NodeB=JrE*adX_Ainv; 30 | 31 | Jacobian_Node{1}=Jacobian_NodeA; 32 | Jacobian_Node{2}=Jacobian_NodeB; 33 | 34 | 35 | end 36 | 37 | -------------------------------------------------------------------------------- /Factor/RelativePose3_consX_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = RelativePose3_consX_Factor( Nodes_array , Measurement_values ) 2 | 3 | NodeA=Nodes_array{1}; 4 | NodeB=Nodes_array{2}; 5 | 6 | 7 | R_A=NodeA(1:3,1:3); p_A=NodeA(1:3,4); 8 | R_B=NodeB(1:3,1:3); p_B=NodeB(1:3,4); 9 | R_u=Measurement_values(1:3,1:3); p_u=Measurement_values(1:3,4); 10 | 11 | 12 | dR= R_A'*R_B*R_u'; 13 | dP= -R_A'*R_B*R_u'*p_u + R_A'*( p_B-p_A ); 14 | 15 | 16 | dtheta=so3_log( dR ); 17 | dp = jacor_inverse(-dtheta)*dP; 18 | 19 | ErrorVector=[ dtheta; dp ]; 20 | 21 | JrE=Jacobian_Lie_inverse(-ErrorVector); 22 | %JrE=eye(6); 23 | 24 | 25 | X_Ainv=[R_A' -R_A'*p_A ]; 26 | adX_Ainv= adjoint( X_Ainv ) ; 27 | 28 | Jacobian_NodeA=-JrE*adX_Ainv; 29 | Jacobian_NodeB=JrE*adX_Ainv; 30 | 31 | Jacobian_Node{1}=Jacobian_NodeA; 32 | Jacobian_Node{2}=Jacobian_NodeB; 33 | 34 | 35 | end 36 | 37 | -------------------------------------------------------------------------------- /Factor/SetNodeDefaultValue.m: -------------------------------------------------------------------------------- 1 | function [ NodeValue ] = SetNodeDefaultValue( NodeTypeName ) 2 | 3 | flag=0; 4 | 5 | switch NodeTypeName 6 | case 'Pose3' 7 | NodeValue=[eye(3 ) zeros(3,1)]; flag=1; 8 | case 'LPose3' 9 | NodeValue=[eye(3 ) zeros(3,1)]; flag=1; 10 | case 'Pose2' 11 | NodeValue=[eye(2) zeros(2,1)]; flag=1; 12 | case 'Landmark3' 13 | NodeValue=zeros(3,1); flag=1; 14 | case 'Velocity3' 15 | NodeValue=zeros(3,1); flag=1; 16 | case 'IMUbias' 17 | NodeValue=zeros(6,1); flag=1; 18 | case 'Landmark2' 19 | NodeValue=zeros(2,1); flag=1; 20 | case 'ParallaxPoint' 21 | NodeValue=[0;0;1; 0]; flag=1; 22 | end 23 | 24 | if flag==0 25 | sprintf('Warning: you have not defined the default value for the new Node in SetNodeDefaultValue.m') 26 | end 27 | 28 | 29 | 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /Factor/VisionNoLandmark_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionNoLandmark_Factor( Nodes_array , Measurement_values ) 2 | 3 | pose=Nodes_array{1}; 4 | 5 | f=Measurement_values.f; 6 | R=pose(1:3,1:3); 7 | p=pose(1:3,4); 8 | Local= R'*( f - p ); 9 | 10 | 11 | fx = 525.0; 12 | fy = 525.0; 13 | cx0 = 639.5; 14 | cy0 = 479.5; 15 | 16 | x= Local(1); 17 | y= Local(2); 18 | z= Local(3); 19 | 20 | K = [ fx 0; 0 fy]; 21 | 22 | h = K*[x/z; y/z]+ [cx0;cy0]; 23 | ErrorVector= h - Measurement_values.uv; 24 | 25 | dh = K* [z 0 -x; 0 z -y ]/(z^2); 26 | 27 | Jacobian_Node{1}=dh*[ R'*skew(f) -R' ]; 28 | %Jacobian_Node{2}=dh*R'; 29 | 30 | 31 | 32 | end 33 | 34 | -------------------------------------------------------------------------------- /Factor/VisionPoseFixed_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionPoseFixed_Factor( Nodes_array , Measurement_values ) 2 | 3 | 4 | pose = Measurement_values.pose; 5 | uv = Measurement_values.uv; 6 | 7 | 8 | f=Nodes_array{1}; 9 | R=pose(1:3,1:3); 10 | p=pose(1:3,4); 11 | Local= R'*( f - p ); 12 | 13 | fx = 525.0; 14 | fy = 525.0; 15 | cx0 = 639.5; 16 | cy0 = 479.5; 17 | 18 | x= Local(1); 19 | y= Local(2); 20 | z= Local(3); 21 | 22 | K = [ fx 0; 0 fy]; 23 | 24 | h = K*[x/z; y/z]+ [cx0;cy0]; 25 | ErrorVector= h - [uv(1);uv(2)]; 26 | 27 | dh = K* [z 0 -x; 0 z -y ]/(z^2); 28 | 29 | Jacobian_Node{1}=dh*R'; 30 | 31 | 32 | 33 | end 34 | 35 | -------------------------------------------------------------------------------- /Factor/VisionTestPoseFixed_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionTestPoseFixed_Factor( Nodes_array , Measurement_values ) 2 | 3 | % Measurement_values is [u;v]; 4 | 5 | pose = Measurement_values.pose; 6 | uv = Measurement_values.uv; 7 | 8 | 9 | f=Nodes_array{1}; 10 | R=pose(1:3,1:3); 11 | p=pose(1:3,4); 12 | Local= R'*( f - p ); 13 | 14 | 15 | fx = 525.0; 16 | fy = 525.0; 17 | cx0 = 639.5; 18 | cy0 = 479.5; 19 | 20 | 21 | d1 = diag( [ 1/fx, 1/fy] )*( uv - [cx0;cy0] ); 22 | d = [d1;1]; 23 | d = d/norm(d); 24 | 25 | normLocal = norm(Local); 26 | ErrorVector = Local/normLocal - d; 27 | 28 | 29 | dh = 1/normLocal*eye(3)- Local*Local'/normLocal^3; 30 | 31 | 32 | 33 | Jacobian_Node{1}=dh*R'; 34 | 35 | 36 | 37 | end 38 | 39 | -------------------------------------------------------------------------------- /Factor/VisionTest_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = VisionTest_Factor( Nodes_array , Measurement_values ) 2 | 3 | % Measurement_values is [u;v]; 4 | 5 | pose=Nodes_array{1}; 6 | f=Nodes_array{2}; 7 | R=pose(1:3,1:3); 8 | p=pose(1:3,4); 9 | Local= R'*( f - p ); 10 | 11 | 12 | fx = 525.0; 13 | fy = 525.0; 14 | cx0 = 639.5; 15 | cy0 = 479.5; 16 | 17 | 18 | d1 = diag( [ 1/fx, 1/fy] )*( Measurement_values - [cx0;cy0] ); 19 | d = [d1;1]; 20 | d = d/norm(d); 21 | 22 | normLocal = norm(Local); 23 | ErrorVector = Local/normLocal - d; 24 | 25 | 26 | dh = 1/normLocal*eye(3)- Local*Local'/normLocal^3; 27 | 28 | 29 | Jacobian_Node{1}=dh*[ R'*skew(f) -R' ]; 30 | 31 | 32 | Jacobian_Node{2}=dh*R'; 33 | 34 | 35 | 36 | 37 | 38 | end 39 | 40 | -------------------------------------------------------------------------------- /Factor/Vision_Factor.m: -------------------------------------------------------------------------------- 1 | function [ ErrorVector, Jacobian_Node ] = Vision_Factor( Nodes_array , Measurement_values ) 2 | 3 | pose=Nodes_array{1}; 4 | f=Nodes_array{2}; 5 | R=pose(1:3,1:3); 6 | p=pose(1:3,4); 7 | Local= R'*( f - p ); 8 | 9 | 10 | fx = 525.0; 11 | fy = 525.0; 12 | cx0 = 639.5; 13 | cy0 = 479.5; 14 | 15 | x= Local(1); 16 | y= Local(2); 17 | z= Local(3); 18 | 19 | K = [ fx 0; 0 fy]; 20 | 21 | h = K*[x/z; y/z]+ [cx0;cy0]; 22 | ErrorVector= h - Measurement_values; 23 | 24 | dh = K* [z 0 -x; 0 z -y ]/(z^2); 25 | 26 | Jacobian_Node{1}=dh*[ R'*skew(f) -R' ]; 27 | Jacobian_Node{2}=dh*R'; 28 | 29 | 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /Factor/update_state.m: -------------------------------------------------------------------------------- 1 | function [ NodeValue ] = update_state( NodeTypeName, NodeValue, increment ) 2 | 3 | flag=0; 4 | 5 | switch NodeTypeName 6 | case 'Pose3' 7 | R=NodeValue(1:3,1:3); p=NodeValue(1:3,4); incre1=increment(1:3); incre2=increment(4:6); 8 | dR=so3_exp(incre1 ); 9 | newR= dR*R; 10 | newp= dR*p+ Jacobian_Lie(-incre1 )*incre2; 11 | NodeValue=[ newR newp ]; 12 | flag=1; 13 | case 'LPose3' 14 | R=NodeValue(1:3,1:3); p=NodeValue(1:3,4); incre1=increment(1:3); incre2=increment(4:6); 15 | dR=so3_exp(incre1 ); 16 | newR= R*dR; 17 | newp= p+R*Jacobian_Lie(-incre1 )*incre2; 18 | NodeValue=[ newR newp ]; 19 | flag=1; 20 | case 'Pose2' 21 | R=NodeValue(1:2,1:2); p=NodeValue(1:2,3); incre1=increment(1); incre2=increment(2:3); 22 | dR=so2_exp(incre1); 23 | newR=dR*R; 24 | % newp=dR*p+ V_so2( incre1 )*incre2; 25 | newp=p+incre2; 26 | NodeValue=[ newR newp ]; 27 | flag=1; 28 | case 'Landmark3' 29 | NodeValue=NodeValue+increment ; flag=1; 30 | case 'Velocity3' 31 | NodeValue=NodeValue+increment ; flag=1; 32 | 33 | case 'IMUbias' 34 | NodeValue=NodeValue+increment ; flag=1; 35 | case 'Landmark2' 36 | NodeValue=NodeValue+increment ; flag=1; 37 | case 'ParallaxPoint' 38 | n=NodeValue(1:3,1); theta = NodeValue(4,1); 39 | NodeValue= [so3_exp(computeA(n)*increment(1:2,1))*n; theta+ increment(3,1)]; flag=1; 40 | end 41 | 42 | if flag==0 43 | sprintf('Warning: you have to define the update for the new Node in update_state.m') 44 | end 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | end 53 | 54 | -------------------------------------------------------------------------------- /Geometry/Cal_triangulate.m: -------------------------------------------------------------------------------- 1 | function [ Landmarks_values ] = Cal_triangulate( Pose3_values , LandmarkManager ) 2 | 3 | landmarks_names = fields( LandmarkManager ); 4 | num_landmarks = size( landmarks_names, 1); 5 | 6 | for i=1:num_landmarks 7 | landmark_id = landmarks_names{i}; 8 | this_landmark_allobservations = LandmarkManager.(landmark_id); 9 | 10 | posearray = fields( this_landmark_allobservations ); 11 | num_poses = size( posearray, 1 ); 12 | if num_poses>1 13 | landmark_position = Fn_triangulate( Pose3_values, this_landmark_allobservations ); 14 | Landmarks_values.(landmark_id)=landmark_position; 15 | end 16 | end 17 | 18 | 19 | 20 | end 21 | 22 | function landmark_position = Fn_triangulate( Pose3_values, this_landmark_allobservations ) 23 | 24 | posearray = fields( this_landmark_allobservations ); 25 | num_poses = size( posearray, 1 ); 26 | 27 | A = zeros(2*num_poses, 3 ); 28 | B = zeros(2*num_poses, 1 ); 29 | 30 | for i=1:num_poses 31 | pose_id = posearray{i}; 32 | pose_i = Pose3_values.(pose_id); 33 | Ri = pose_i(1:3,1:3); pi = pose_i(1:3,4); 34 | uv = this_landmark_allobservations.(pose_id); 35 | 36 | fx = 525.0; 37 | fy = 525.0; 38 | cx0 = 639.5; 39 | cy0 = 479.5; 40 | K_inv = [ 1/fx 0; 0 1/fy]; 41 | A_i = [ eye(2) -K_inv*(uv - [cx0;cy0] ) ]; 42 | 43 | A(2*i-1:2*i,1:3) =A_i*Ri'; 44 | B(2*i-1:2*i,:) = A_i*Ri'*pi; 45 | 46 | end 47 | 48 | landmark_position = pinv(A)*B; 49 | 50 | 51 | end -------------------------------------------------------------------------------- /Geometry/Fn_GetLandmarkCoordinate.m: -------------------------------------------------------------------------------- 1 | function [ landmark_coordinate ] = Fn_GetLandmarkCoordinate( ancor_main, ancor_au, landmark_para ) 2 | 3 | Rm = ancor_main(1:3,1:3); pm= ancor_main(1:3,4); 4 | Ra = ancor_au(1:3,1:3); pa= ancor_au(1:3,4); 5 | 6 | 7 | n_m = landmark_para(1:3); 8 | theta = landmark_para(4); 9 | 10 | n = Rm*n_m; 11 | 12 | t = pm-pa; 13 | norm_t = norm(t); 14 | if norm_t > 0.000001 15 | alpha = acos((t'*n)/norm_t); 16 | else 17 | alpha = 0; 18 | end 19 | 20 | 21 | d = norm_t*sin(alpha-theta)/sin(theta); % alpha> theta 22 | 23 | 24 | 25 | landmark_coordinate = d*n + pm; 26 | 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /Geometry/Fn_LandmarkManager.m: -------------------------------------------------------------------------------- 1 | function [ LandmarkManager ] = Fn_LandmarkManager( Measurement_Vision ) 2 | 3 | sizem = size( Measurement_Vision, 1); 4 | 5 | for i = 1:sizem 6 | 7 | landmark_id = ['landmark' num2str(Measurement_Vision(i,2) ) ]; 8 | pose_id = ['pose' num2str(Measurement_Vision(i,1) ) ]; 9 | uv = Measurement_Vision(i,3:4)'; 10 | LandmarkManager.(landmark_id).(pose_id)= uv; 11 | 12 | end 13 | 14 | 15 | end 16 | 17 | -------------------------------------------------------------------------------- /Geometry/Fn_vectorLandmarkP3.m: -------------------------------------------------------------------------------- 1 | function [ vector, Jacobian_vector ] = Fn_vectorLandmarkP3( ancor_main, ancor_au, pose, landmark_para ) 2 | 3 | Rm = ancor_main(1:3,1:3); pm= ancor_main(1:3,4); 4 | Ra = ancor_au(1:3,1:3); pa= ancor_au(1:3,4); 5 | R = pose(1:3,1:3); p = pose(1:3,4); 6 | 7 | n_m = landmark_para(1:3); % unit vector 8 | theta = landmark_para(4); % parallex angle < alpha 9 | 10 | 11 | 12 | n = Rm*n_m; 13 | p2p1= pm-pa; 14 | 15 | vector_1 = sqrt( skew(p2p1 )* n )* n; 16 | vector_2 = ( p2p1'*n) * n; 17 | vector_3 = sin(theta)*( pm-p ); 18 | 19 | vector = vector_1 - vector_2 + vector_3; 20 | 21 | 22 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 23 | %vector_1 = sqrt( skew(p2p1 )* n )* n; 24 | 25 | dVector_1_dRm= 26 | dVector_1_dpm= 27 | dVector_1_dposem = [dVector_1_dRm dVector_1_dpm]; 28 | 29 | 30 | dVector_1_dRa= 31 | dVector_1_dpa= 32 | dVector_1_dposea = [dVector_1_dRa dVector_1_dpa]; 33 | 34 | dVector_1_dR= 35 | dVector_1_dp= 36 | dVector_1_dpose = [dVector_1_dR dVector_1_dp]; 37 | 38 | 39 | dVector_1_dn = 40 | dVector_1_dtheta = zeros(3,1); 41 | dVector_1_landmark = [ dVector_1_dn dVector_1_dtheta ]; 42 | 43 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 44 | %vector_2 = ( p2p1'*n) * n; 45 | dVector_2_dRm= 46 | dVector_2_dpm= 47 | dVector_2_dposem = [dVector_2_dRm dVector_2_dpm]; 48 | 49 | 50 | dVector_2_dRa= 51 | dVector_2_dpa= 52 | dVector_2_dposea = [dVector_2_dRa dVector_2_dpa]; 53 | 54 | 55 | dVector_2_dR= 56 | dVector_2_dp= 57 | dVector_2_dpose = [dVector_2_dR dVector_2_dp]; 58 | 59 | 60 | 61 | dVector_2_dn = 62 | dVector_2_dtheta = zeros(3,1); 63 | dVector_2_landmark = [ dVector_2_dn dVector_2_dtheta ]; 64 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 65 | %vector_3 = sin(theta)*( pm-p ); 66 | dVector_3_dRm= -sin(theta)*skew(pm); 67 | dVector_3_dpm= sin(theta)*eye(3); 68 | dVector_3_dposem = [dVector_3_dRm dVector_3_dpm]; 69 | 70 | 71 | dVector_3_dRa= zeros(3,3); 72 | dVector_3_dpa= zeros(3,3); 73 | dVector_3_dposea = [dVector_3_dRa dVector_3_dpa]; 74 | 75 | dVector_3_dR= sin(theta)*skew(p); 76 | dVector_3_dp= -sin(theta)*eye(3); 77 | dVector_3_dpose = [dVector_3_dR dVector_3_dp]; 78 | 79 | 80 | dVector_3_dn = zeros(3,2); 81 | dVector_3_dtheta = cos(theta)*( pm-p ); 82 | dVector_3_landmark = [ dVector_3_dn dVector_3_dtheta ]; 83 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 84 | 85 | dVector_dposem = dVector_1_dposem-dVector_2_dposem+ dVector_3_dposem; 86 | dVector_dposea = dVector_1_dposea-dVector_2_dposea+ dVector_3_dposea; 87 | dVector_dpose = dVector_1_dpose-dVector_2_dpose+ dVector_3_dpose; 88 | dVector_dlandmark = dVector_1_landmark-dVector_2_landmark+dVector_3_landmark; 89 | 90 | 91 | 92 | end 93 | 94 | -------------------------------------------------------------------------------- /Geometry/GetParallexFromTwoPoses.m: -------------------------------------------------------------------------------- 1 | function [ nAndtheta ] = GetParallexFromTwoPoses( pose1, pose2, uvArray ) 2 | 3 | R1 = pose1(1:3,1:3); p1=pose1(1:3,4); 4 | R2 = pose2(1:3,1:3); p2=pose2(1:3,4); 5 | 6 | 7 | fx = 525.0; 8 | fy = 525.0; 9 | cx0 = 639.5; 10 | cy0 = 479.5; 11 | 12 | uv1 = uvArray{1}; 13 | uv2 = uvArray{2}; 14 | 15 | d1 = [[1/fx 0; 0 1/fy]*(uv1 - [cx0;cy0]); 1]; 16 | d1 = d1/norm(d1); 17 | 18 | d2 = [[1/fx 0; 0 1/fy]*(uv2 - [cx0;cy0]); 1]; 19 | d2 = d2/norm(d2); 20 | 21 | n1 = R1*d1; % in world frame 22 | n2 = R2*d2; % in world frame 23 | 24 | n = d1; 25 | theta =acos(n1'*n2); 26 | 27 | 28 | nAndtheta = [n; theta]; 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /Geometry/logmap_parallax.m: -------------------------------------------------------------------------------- 1 | function [ y ] = logmap_parallax( x1, x2 ) 2 | % x1 - x2 3 | 4 | 5 | y = zeros(3,1); 6 | 7 | n1 = x1(1:3); theta1 = x1(4); 8 | n2 = x2(1:3); theta2 = x2(4); 9 | 10 | 11 | angle = acos(n1'*n2); 12 | 13 | if angle > 0.000001 14 | n3 = skew(n2)*n1; 15 | n3 = n3/norm(n3); 16 | A = null(n2'); 17 | y(1:2) = inv(A(1:2,1:2))*n3(1:2)*angle; 18 | else 19 | 20 | y(1:2) = [0;0]; 21 | end 22 | y(3) = theta1 - theta2; 23 | 24 | end 25 | 26 | -------------------------------------------------------------------------------- /Math/Jaco_r_se3_inverse.m: -------------------------------------------------------------------------------- 1 | function [ y ] = Jaco_r_se3_inverse( E ) 2 | 3 | e1=E(1:3); 4 | e2=E(4:6); 5 | 6 | j1=jacor_inverse(e1); 7 | Qr2=Kr( e1, e2 ); 8 | 9 | 10 | y= [ j1 zeros(3,3);... 11 | -j1*Qr2*j1 j1 ]; 12 | 13 | 14 | 15 | end 16 | 17 | -------------------------------------------------------------------------------- /Math/Jacobian_Lie.m: -------------------------------------------------------------------------------- 1 | function [ Y ] = Jacobian_Lie( x ) 2 | 3 | 4 | sizex=size(x,1); 5 | 6 | switch sizex 7 | case 3 8 | x1=x(1:3); Y= jaco_r( x1 ); 9 | case 6 10 | x1=x(1:3); x2=x(4:6); Y= [ jaco_r( x1 ) zeros(3,3); Kr(x1,x2) jaco_r( x1 ) ] ; 11 | case 9 12 | x1=x(1:3); x2=x(4:6); x3=x(7:9); Y= [ jaco_r( x1 ) zeros(3,6); Kr(x1,x2) jaco_r( x1 ) zeros(3,3); Kr(x1,x3) zeros(3,3) jaco_r( x1 ) ] ; 13 | end 14 | 15 | 16 | 17 | end 18 | 19 | -------------------------------------------------------------------------------- /Math/Jacobian_Lie_inverse.m: -------------------------------------------------------------------------------- 1 | function [ Y ] = Jacobian_Lie_inverse( x ) 2 | 3 | sizex=size(x,1); 4 | 5 | switch sizex 6 | case 3 7 | x1=x(1:3); Y= jacor_inverse( x1 ); 8 | case 6 9 | x1=x(1:3); x2=x(4:6); j1=jacor_inverse( x1 ); Q=Kr(x1,x2); Y= [ j1 zeros(3,3); -j1*Q*j1 j1 ]; 10 | case 9 11 | x1=x(1:3); x2=x(4:6);x3=x(7:9); 12 | j1=jacor_inverse( x1 ); 13 | Q2=Kr(x1,x2); 14 | Q3=Kr(x1,x3); 15 | Y= [ j1 zeros(3,6); -j1*Q2*j1 j1 zeros(3,3); -j1*Q3*j1 zeros(3,3) j1]; 16 | end 17 | 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /Math/Jr.m: -------------------------------------------------------------------------------- 1 | function [ Jre ] = Jr( e ) 2 | 3 | e1=e(1:3); 4 | e2=e(4:6); 5 | e3=e(7:9); 6 | 7 | jr1= jaco_r( e1 ); 8 | kr2= Kr(e1,e2); 9 | kr3= Kr(e1,e3); 10 | 11 | Jre= [jr1 zeros(3,6) ;... 12 | kr2 jr1 zeros(3,3);... 13 | kr3 zeros(3,3) jr1 ]; 14 | 15 | end 16 | 17 | -------------------------------------------------------------------------------- /Math/Jr_inverse.m: -------------------------------------------------------------------------------- 1 | function [ Jreinvserse ] = Jr_inverse( e ) 2 | 3 | e1=e(1:3); 4 | e2=e(4:6); 5 | e3=e(7:9); 6 | 7 | j1=jacor_inverse(e1); 8 | Qr2=Kr( e1, e2 ); 9 | Qr3=Kr( e1, e3 ); 10 | 11 | Jreinvserse=[ j1 zeros(3,3) zeros(3,3);... 12 | -j1*Qr2*j1 j1 zeros(3,3);... 13 | -j1*Qr3*j1 zeros(3,3) j1 ]; 14 | 15 | 16 | 17 | 18 | 19 | 20 | end 21 | 22 | -------------------------------------------------------------------------------- /Math/Kr.m: -------------------------------------------------------------------------------- 1 | function [ Kr_value ] = Kr( theta1, theta2 ) 2 | 3 | 4 | Kr_value=Kl(-theta1,-theta2); 5 | 6 | 7 | 8 | 9 | end 10 | 11 | 12 | function Kl_value=Kl(x1,x2) 13 | 14 | 15 | theta=norm(x1); 16 | 17 | if theta > 0.00001 18 | Kl_value=1/2*skew(x2)+(theta-sin(theta))/theta^3*(skew(x1)*skew(x2)+skew(x2)*skew(x1)+skew(x1)*skew(x2)*skew(x1))... 19 | -(1-theta^2/2-cos(theta))/theta^4*(skew(x1)*skew(x1)*skew(x2)+skew(x2)*skew(x1)*skew(x1)-3*skew(x1)*skew(x2)*skew(x1) )... 20 | -1/2*( (1-theta^2/2-cos(theta))/theta^4-3*(theta-sin(theta)-theta^3/6)/theta^5)*(skew(x1)*skew(x2)*skew(x1)*skew(x1)+skew(x1)*skew(x1)*skew(x2)*skew(x1)); 21 | 22 | else 23 | Kl_value= 1/2*skew(x2); 24 | end 25 | 26 | 27 | 28 | end -------------------------------------------------------------------------------- /Math/PBA_Jacobian.m: -------------------------------------------------------------------------------- 1 | function [fc, J_m , J_a, J_i , J_f] = PBA_Jacobian( pose_m, pose_a, pose, feature, Tcb ) 2 | right =0; 3 | Rcb = Tcb(1:3,1:3); 4 | tcb = Tcb(1:3,4); 5 | 6 | Rbc = Rcb'; 7 | tbc = -Rcb'*tcb; 8 | 9 | 10 | 11 | Rm=pose_m(1:3,1:3); pm=pose_m(1:3,4); 12 | Ra=pose_a(1:3,1:3); pa=pose_a(1:3,4); 13 | R=pose(1:3,1:3); p=pose(1:3,4); 14 | n1=feature(1:3,1); theta = feature(4,1); 15 | 16 | pmc = pm+Rm*tbc; 17 | pac = pa+Ra*tbc; 18 | pc = p+ R*tbc; 19 | 20 | a = pmc-pac; 21 | n_m = Rm*Rbc*n1; 22 | b= pmc- pc; 23 | 24 | 25 | 26 | 27 | N = cos(theta)*norm(skew(a)*n_m)*n_m +sin(theta)*( b-(a'*n_m)*n_m); 28 | fc = Rcb* R'*N; 29 | 30 | [ da_m, da_a, da_f] = derivative_a( Rm, pm, Ra, pa, n1, theta , right, Rbc, tbc ) ; 31 | [ dnm_m, dnm_a, dnm_f] = derivative_nm( Rm, pm, Ra, pa, n1, theta , right, Rbc, tbc ); 32 | [ db_m, db_a, db_f] = derivative_b( Rm, pm, Ra, pa, n1, theta , right, Rbc, tbc ); 33 | 34 | normCrossProduct=norm(skew(a)*n_m); 35 | 36 | N1 = cos(theta)*( normCrossProduct*eye(3)+n_m*(skew(a)*n_m)'/normCrossProduct*skew(a))... 37 | -sin(theta)*(n_m*a'+(a'*n_m)*eye(3)); 38 | 39 | N2 = -cos(theta)*n_m*(skew(a)*n_m)'/normCrossProduct*skew(n_m)-sin(theta)*(n_m*n_m'); 40 | 41 | dN_m = N1*dnm_m+ N2*da_m+sin(theta)*db_m; 42 | dN_a = N1*dnm_a+ N2*da_a+sin(theta)*db_a; 43 | dN_f = N1*dnm_f+ N2*da_f+sin(theta)*db_f+... 44 | (b-(a'*n_m)*n_m)*[0 0 cos(theta)]+normCrossProduct*n_m*[0 0 -sin(theta)]; 45 | 46 | 47 | J_m =Rcb* R'*dN_m; 48 | J_a = Rcb* R'*dN_a; 49 | peudo_f = N + sin(theta)*p; 50 | J_f = Rcb*R'*dN_f; 51 | 52 | if right ==1 53 | J_i = Rcb*[ R'*skew(peudo_f) -sin(theta)*R']; 54 | elseif right ==0 55 | J_i = Rcb*[skew(Rbc*fc+sin(theta)*tbc) -sin(theta)*eye(3)]; 56 | end 57 | 58 | 59 | 60 | end 61 | 62 | 63 | 64 | 65 | 66 | function [ da_m, da_a, da_f] = derivative_a( Rm, pm, Ra, pa, n1, theta , right, Rbc, tbc ) 67 | if right == 1 68 | da_m = [-skew(pm) eye(3)]; 69 | da_a = -[-skew(pa) eye(3)]; 70 | da_f = zeros(3,3); 71 | 72 | else 73 | 74 | da_m = [-Rm*skew(tbc) Rm]; 75 | da_a = [Ra*skew(tbc) -Ra]; 76 | da_f = zeros(3,3); 77 | 78 | end 79 | 80 | end 81 | 82 | function [ dnm_m, dnm_a, dnm_f] = derivative_nm( Rm, pm, Ra, pa, n1, theta , right, Rbc, tbc ) 83 | if right == 1 84 | dnm_m = [-skew(Rm*n1) zeros(3,3)]; 85 | dnm_a = zeros(3,6); 86 | dnm_f = -Rm*skew(n1)*[computeA(n1) zeros(3,1)]; 87 | 88 | else 89 | dnm_m = [-Rm*skew(Rbc*n1) zeros(3,3)]; 90 | dnm_a = zeros(3,6); 91 | dnm_f = -Rm*Rbc*skew(n1)*[computeA(n1) zeros(3,1)]; 92 | 93 | 94 | 95 | end 96 | 97 | 98 | end 99 | 100 | function [ db_m, db_a, db_f] = derivative_b( Rm, pm, Ra, pa, n1, theta , right, Rbc, tbc ) 101 | if right == 1 102 | db_m = [-skew(pm) eye(3)]; 103 | db_a = zeros(3,6); 104 | db_f = zeros(3,3); 105 | 106 | else 107 | 108 | db_m = [-Rm*skew(tbc) Rm]; 109 | db_a = zeros(3,6); 110 | db_f = zeros(3,3); 111 | 112 | 113 | end 114 | 115 | 116 | end -------------------------------------------------------------------------------- /Math/SE23.m: -------------------------------------------------------------------------------- 1 | function [ y ] = SE23( EV ) 2 | 3 | y = [so3_exp(EV(1:3)) jaco_r(-EV(1:3))*EV(4:6) jaco_r(-EV(1:3))*EV(7:9)]; 4 | 5 | 6 | y = [y ; 0 0 0 1 0; 0 0 0 0 1]; 7 | 8 | end 9 | 10 | -------------------------------------------------------------------------------- /Math/TestJacobian.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | v1 = rand(9,1); 4 | v2 = randn(9,1)*0.01; 5 | v2 = ones(9,1)*0.01; 6 | 7 | [ V1 ] = SE23( v1 ); 8 | V2 = SE23( v2); 9 | 10 | norm(V1*V2 - SE23( v1 + Jr_inverse(v1)*v2 )) 11 | norm(V1*V2 - SE23( v1 + v2 )) 12 | 13 | 14 | norm(V2*V1 -SE23( v1+ Jr_inverse(-v1)*v2 ) ) -------------------------------------------------------------------------------- /Math/Test_Jr.m: -------------------------------------------------------------------------------- 1 | clear 2 | clc 3 | 4 | x= [0.2;0.3;-0.4; randn(3,1)]; 5 | 6 | 7 | 8 | dx=0.0001*ones(6,1); 9 | 10 | %se3_log( se3_exp(x) ) -x 11 | %SE3_x= se3_log( se3_group(se3_exp(x), se3_exp( Jacobian_Lie(x) * dx) )) - x 12 | %SE3_x= se3_log( se3_group(se3_exp(x), se3_exp( dx) )) - Jacobian_Lie_inverse( x )*dx- x 13 | SE3_x= se3_log( se3_group(se3_exp(x), se3_exp( dx) )) - dx- x -------------------------------------------------------------------------------- /Math/V_so2.m: -------------------------------------------------------------------------------- 1 | function [ y ] = V_so2( x ) 2 | 3 | t = norm(x); 4 | 5 | if t < 0.0001 6 | y=eye(2); 7 | else 8 | y= 1/t * [ sin(t) cos(t)-1; 1-cos(t) sin(t) ]; 9 | end 10 | 11 | 12 | end 13 | 14 | -------------------------------------------------------------------------------- /Math/adjoint.m: -------------------------------------------------------------------------------- 1 | function [ y ] = adjoint( X ) 2 | 3 | y=0; 4 | sizeX=size(X,2); 5 | 6 | 7 | switch sizeX 8 | case 3 9 | R=X(1:3,1:3); y=R; 10 | case 4 11 | R=X(1:3,1:3); p=X(1:3,4); y=[ R zeros(3,3); skew(p)*R R ]; 12 | case 5 13 | R=X(1:3,1:3); v=X(1:3,4); p=X(1:3,5); y=[ R zeros(3,6); skew(v)*R R zeros(3,3); skew(p)*R zeros(3,3) R ]; 14 | end 15 | 16 | 17 | end 18 | 19 | -------------------------------------------------------------------------------- /Math/computeA.m: -------------------------------------------------------------------------------- 1 | function [ A ] = computeA( n ) 2 | 3 | 4 | A = null(n'); 5 | 6 | 7 | end 8 | 9 | -------------------------------------------------------------------------------- /Math/jaco_r.m: -------------------------------------------------------------------------------- 1 | function result = jaco_r( delta ) 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % Function: Overloading function. 4 | % Method1: I(3x3) - (1-cos(theta))*skew(delta)/norm(delta)^2 + ... 5 | % (norm(delta)-sin(norm(delta)))*skew(delta)^2/norm(delta)^3 6 | % Input1: delta: 3x1 vector 7 | % Returns1: result: See the document 8 | 9 | % Method2: See the document 10 | % Input2: delta: 6+3N vector 11 | % Returns2: result: See the document 12 | 13 | % Author: Jingwei Song. 04/06/2016 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | N= (size(delta,1)-3)/3; 16 | 17 | if(size(delta,1) == 3) 18 | function_mode = 1; 19 | else 20 | function_mode = 2; 21 | end 22 | 23 | if(function_mode == 1) 24 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 25 | % Function1 26 | theta = norm(delta); 27 | if(theta < 0.00000001) 28 | result = eye(3,3); 29 | else 30 | result = eye(3,3) - (1-cos(theta))*skew(delta)/theta^2 + (theta-sin(theta))*skew(delta)^2/norm(delta)^3; 31 | end 32 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | else 34 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 35 | % Function2 36 | result = zeros(3+3*N,3+3*N); 37 | s_theta = delta(1:3,1); 38 | for i = 1 : N+1 % Set diagonal to J_r(s_theta) 39 | result(3*i-2:3*i,3*i-2:3*i) = jaco_r(s_theta); 40 | end 41 | for i = 1 : N 42 | s_temp = delta(3*i+1:3*i+3,1); 43 | result(3*i+1:3*i+3,1:3) = K_r_x1x2(s_theta, s_temp); 44 | end 45 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 46 | end 47 | 48 | 49 | end 50 | 51 | -------------------------------------------------------------------------------- /Math/jacor_inverse.m: -------------------------------------------------------------------------------- 1 | function [ jrx_inverse ] = jacor_inverse( dx ) 2 | phi=sqrt(dx'*dx); 3 | 4 | if phi<0.00001 5 | jrx_inverse=eye(3); 6 | else 7 | sp=skew(dx); 8 | jrx_inverse=eye(3)+0.5*sp+(1/phi^2 - (1+cos(phi))/(2*phi*sin(phi)))*sp*sp; 9 | end 10 | 11 | -------------------------------------------------------------------------------- /Math/se23_log.m: -------------------------------------------------------------------------------- 1 | function [ y ] = se23_log( X ) 2 | 3 | 4 | R= X(1:3,1:3); 5 | v= X(1:3,4); 6 | p= X(1:3,5); 7 | 8 | theta = so3_log(R); 9 | 10 | dv=jacor_inverse(-theta)*v; 11 | 12 | dp=jacor_inverse(-theta)*p; 13 | 14 | y=[theta; dv; dp]; 15 | 16 | 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /Math/se3_exp.m: -------------------------------------------------------------------------------- 1 | function [ y ] = se3_exp( x ) 2 | 3 | x1=x(1:3); 4 | x2=x(4:6); 5 | 6 | R= so3_exp(x1); 7 | p= jaco_r(-x1)*x2; 8 | 9 | 10 | y=[R p]; 11 | 12 | 13 | 14 | 15 | end 16 | 17 | -------------------------------------------------------------------------------- /Math/se3_group.m: -------------------------------------------------------------------------------- 1 | function [ y ] = se3_group( X1, X2 ) 2 | 3 | C1= [X1; 0 0 0 1]; 4 | C2= [X2; 0 0 0 1]; 5 | 6 | C=C1*C2; 7 | 8 | 9 | y=C(1:3,1:4); 10 | 11 | 12 | 13 | end 14 | 15 | -------------------------------------------------------------------------------- /Math/se3_log.m: -------------------------------------------------------------------------------- 1 | function [ y ] = se3_log( x ) 2 | R= x(1:3,1:3); 3 | p= x(1:3,4); 4 | 5 | 6 | theta = so3_log(R); 7 | 8 | dp=jacor_inverse(-theta)*p; 9 | 10 | 11 | y=[theta; dp]; 12 | 13 | 14 | end 15 | 16 | -------------------------------------------------------------------------------- /Math/skew.m: -------------------------------------------------------------------------------- 1 | function y = skew( x ) 2 | % compute the skew symetric matrix 3 | % given input 3x1 vector x 4 | y=[0 -x(3) x(2);... 5 | x(3) 0 -x(1);... 6 | -x(2) x(1) 0]; 7 | end 8 | 9 | -------------------------------------------------------------------------------- /Math/so2_exp.m: -------------------------------------------------------------------------------- 1 | function [ y ] = so2_exp( x ) 2 | 3 | y= [ cos(x) -sin(x); sin(x) cos(x) ]; 4 | 5 | 6 | 7 | 8 | end 9 | 10 | -------------------------------------------------------------------------------- /Math/so3_exp.m: -------------------------------------------------------------------------------- 1 | function [ result ] = so3_exp( x ) 2 | % compute the exponential mapping of R^3 to SO(3) 3 | % use sparse matrix 4 | N = size(x,1); 5 | %if N == 3 6 | theta=norm(x); 7 | if theta==0 8 | result=eye(3); 9 | else 10 | omega =x/theta; 11 | result=eye(3,3) + sin(theta) * skew(omega) + (1 - cos(theta))*skew(omega)^2; 12 | end 13 | 14 | %else 15 | % todo, compute exp() of R^6 to SE(3)? 16 | % m = (N-3)/3; 17 | % result=speye(3+m); 18 | % result(1:3,1:3)=Exp( x(1:3)); 19 | %end 20 | end 21 | 22 | -------------------------------------------------------------------------------- /Math/so3_hatinv.m: -------------------------------------------------------------------------------- 1 | function v = so3_hatinv(xi) 2 | v = [xi(3,2); xi(1,3); xi(2,1)]; -------------------------------------------------------------------------------- /Math/so3_log.m: -------------------------------------------------------------------------------- 1 | function f = so3_log(R, varargin) 2 | 3 | if (nargin>1) 4 | if (norm(R-eye(3),'fro') < 2*eps) 5 | f = zeros(3,1); 6 | return 7 | end 8 | end 9 | 10 | phi = acos(1/2*(trace(R)-1)); 11 | 12 | if (nargin>1) 13 | if (abs(phi) < 1e-10) 14 | f = zeros(3,1); 15 | return 16 | end 17 | end 18 | 19 | if norm(R-eye(3))>0.00001 20 | f = so3_hatinv(phi/(2*sin(phi))*(R-R')); 21 | else 22 | f=[0;0;0]; 23 | end -------------------------------------------------------------------------------- /Math/special_add_left.m: -------------------------------------------------------------------------------- 1 | function [ X ] = special_add_left( X, S ) 2 | % add on Lie group-Lie Algebra space 3 | s_theta=S(1:3); 4 | s_p=S(4:6); 5 | sizeS=size(S,1); 6 | NumberOfLandmarks=(sizeS-6)/3; 7 | X.position=X.position+X.orientation*jaco_r(-s_theta)*s_p; 8 | if NumberOfLandmarks>=1 9 | s_landmarksMatrix=reshape(S(7:end),3,NumberOfLandmarks); 10 | X.landmarks(1:3,:)=X.landmarks(1:3,:)+X.orientation*jaco_r(-s_theta)*s_landmarksMatrix; 11 | end 12 | X.orientation=X.orientation*so3_exp(s_theta); 13 | end 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /Math/special_add_right.m: -------------------------------------------------------------------------------- 1 | function [ X ] = special_add_right( X,S ) 2 | 3 | s_theta=S(1:3); 4 | s_p=S(4:6); 5 | 6 | sizeS=size(S,1); 7 | NumberOfLandmarks=(sizeS-6)/3; 8 | 9 | Exps=so3_exp(s_theta); 10 | 11 | X.position= Exps*X.position+jaco_r(-s_theta)*s_p; 12 | 13 | 14 | 15 | if NumberOfLandmarks>=1 16 | s_landmarksMatrix=reshape(S(7:end),3,NumberOfLandmarks); 17 | X.landmarks(1:3,:)=Exps*X.landmarks(1:3,:)+jaco_r(-s_theta)*s_landmarksMatrix; 18 | end 19 | 20 | X.orientation=Exps*X.orientation; 21 | 22 | 23 | end -------------------------------------------------------------------------------- /NewExample_VINS_visual.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | addpath('./Geometry/'); 8 | addpath('./VINS_DataGenerator/'); 9 | 10 | load VINS_cubic3.mat 11 | 12 | [ Graph ] = InitializeGraph; 13 | Graph.Fixed.IDname.pose0 =1; 14 | Graph.Fixed.IDname.pose1 =1; 15 | 16 | 17 | num_poses = 49; 18 | 19 | ss=1; 20 | while( Measurement_Vision(ss,1)<= num_poses) 21 | 22 | ss = ss+1; 23 | 24 | end 25 | 26 | i=1; 27 | 28 | 29 | 30 | while( Measurement_Vision(i,1)<= num_poses) 31 | NodeArray=[]; 32 | pose_id = ['pose' num2str( Measurement_Vision(i,1) )]; 33 | pose_index = Measurement_Vision(i,1); 34 | landmark_id =['landmark' num2str( Measurement_Vision(i,2) )]; 35 | Measurement.value= (Measurement_Vision(i,3:4))' ; 36 | depth_rough = norm(RGBD_data(i,3:5))*(1+randn*0); 37 | %Measurement.inf= (1/depth_rough)^2*eye(2)*(525^2); 38 | Measurement.inf=eye(3); 39 | 40 | NodeArray{1,1}='Pose3'; NodeArray{1,2}=pose_id; 41 | NodeArray{2,1}='Landmark3'; NodeArray{2,2}=landmark_id; 42 | 43 | 44 | if isfield(Graph.Nodes, 'Landmark3' ) 45 | if isfield(Graph.Nodes.Landmark3.Values, landmark_id) 46 | kk =1 ; 47 | else kk=0; 48 | end 49 | else kk=0; 50 | end 51 | 52 | num_landmark_id = Measurement_Vision(i,2); 53 | num_ob_id = sum(Measurement_Vision(1:ss-1,2)==num_landmark_id); 54 | 55 | if num_ob_id>=2 56 | if 1 57 | [ Graph ]= AddComplexEdge(Graph, 'VisionTest_Factor', NodeArray, Measurement); 58 | if kk == 1 59 | Graph.Nodes.Landmark3.Values.(landmark_id )= Landmarks.(landmark_id)+1.3*randn(3,1); 60 | 61 | end 62 | end 63 | pose = RobotState{pose_index+1}.pose; 64 | R = pose(1:3,1:3)*expm(skew( randn(3,1)*.1)); 65 | t = pose(1:3,4)+randn(3,1)*1.2; 66 | 67 | Graph.Nodes.Pose3.Values.(pose_id)=[ R t ]; 68 | end 69 | 70 | 71 | i=i+1; 72 | end 73 | Graph.Nodes.Pose3.Values.pose0 = RobotState{1}.pose; 74 | Graph.Nodes.Pose3.Values.pose1 = RobotState{2}.pose; 75 | 76 | 77 | S_max= find(Measurement_Vision(:,1)== num_poses , 1, 'last' ); 78 | 79 | [ LandmarkManager ] = Fn_LandmarkManager( Measurement_Vision(1: S_max, :) ); 80 | [ Landmarks_values ] = Cal_triangulate( Graph.Nodes.Pose3.Values , LandmarkManager ); 81 | %Graph.Nodes.Landmark3.Values=Landmarks_values; 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | subplot(1,3,1); 93 | PlotTrajectory( Graph ); 94 | title('Odometry And Triangulate'); 95 | 96 | % load gns_Linear.mat; 97 | % Graph.Nodes = GraphNodes; 98 | tic 99 | [ Graph ] = PerformGO_LM( Graph ); 100 | toc 101 | axis([-5 5 -5 5 -5 5]*8/5) 102 | subplot(1,3,2) 103 | PlotTrajectory( Graph ); 104 | title('Optimization'); 105 | axis([-5 5 -5 5 -5 5]*8/5) 106 | 107 | subplot(1,3,3) 108 | PlotTrajectory_Ground( RobotState, num_poses ); 109 | axis([-5 5 -5 5 -5 5]*8/5) 110 | 111 | 112 | 113 | 114 | -------------------------------------------------------------------------------- /PBAexample/AddInitialGuessForPoses.m: -------------------------------------------------------------------------------- 1 | function [Graph] = AddInitialGuessForPoses(Graph, filename) 2 | 3 | load (filename); 4 | 5 | all_Pose_name = fields(Graph.Nodes.LPose3.Values); 6 | num_poses = size( all_Pose_name,1); 7 | 8 | for i = 1: num_poses 9 | 10 | thisPose_name = all_Pose_name{i}; 11 | SS =regexp(thisPose_name,'e','split'); 12 | index_pose = SS{2}; 13 | pose_No = str2num( index_pose ); 14 | 15 | 16 | pose = RobotState{pose_No+1}.pose; 17 | 18 | if pose_No~=0 && pose_No~=1 19 | R = pose(1:3,1:3)*expm(skew( randn(3,1)*0.1)); 20 | t = pose(1:3,4)+randn(3,1)*0.2; 21 | pose = [R t]; 22 | end 23 | 24 | Graph.Nodes.LPose3.Values.(thisPose_name)=pose; 25 | 26 | end 27 | 28 | 29 | end 30 | 31 | -------------------------------------------------------------------------------- /PBAexample/GeneratePBALandmarkManager.m: -------------------------------------------------------------------------------- 1 | function [ NewLandmarkManager ] = GeneratePBALandmarkManager( filename , num_poses) 2 | 3 | 4 | load (filename); 5 | 6 | 7 | 8 | S_max= find(Measurement_Vision(:,1)== num_poses , 1, 'last' ); 9 | [ LandmarkManager ] = Fn_LandmarkManager( Measurement_Vision(1: S_max, :) ); 10 | 11 | all_landmarkNames = fields(LandmarkManager); 12 | num_landmarks =size(all_landmarkNames,1); 13 | 14 | % delete all landmarks observed only once 15 | NewLandmarkManager = struct; 16 | for i=1:num_landmarks 17 | name_i = all_landmarkNames{i}; 18 | 19 | numOBs_thisLandmark = size(fields(LandmarkManager.(name_i)),1); 20 | 21 | if numOBs_thisLandmark>2 22 | NewLandmarkManager.(name_i) = LandmarkManager.(name_i); 23 | end 24 | end 25 | 26 | 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /PBAexample/MakeAnchorAndGenerateInitial.m: -------------------------------------------------------------------------------- 1 | function NewLandmarkManager = MakeAnchorAndGenerateInitial( LandmarkManager, filename ) 2 | 3 | 4 | all_landmarkNames = fields(LandmarkManager); 5 | num_landmarks =size(all_landmarkNames,1); 6 | 7 | 8 | for i = 1: num_landmarks 9 | 10 | name_i = all_landmarkNames{i}; 11 | landmark_i_info = LandmarkManager.(name_i); 12 | Newlandmark_i_info = MakeAnchorAndGenerateInitial_ForEachLandmark(landmark_i_info, filename); 13 | 14 | NewLandmarkManager.(name_i) = Newlandmark_i_info; 15 | 16 | end 17 | 18 | 19 | 20 | end 21 | 22 | function landmark_i_info = MakeAnchorAndGenerateInitial_ForEachLandmark(landmark_i_info, filename) 23 | 24 | load (filename); 25 | 26 | all_PosesNames= fields(landmark_i_info); 27 | num_Poses = size(all_PosesNames,1); 28 | 29 | main_Anchor = all_PosesNames{1}; 30 | landmark_i_info.MainAnchor = main_Anchor; 31 | 32 | 33 | 34 | SS =regexp(main_Anchor,'e','split'); 35 | index_pose = SS{2}; 36 | index_pose = str2num( index_pose ); 37 | pose_m = RobotState{index_pose+1}.pose; 38 | uvArray{1} = landmark_i_info.(main_Anchor); 39 | tempuvArray{1} = landmark_i_info.(main_Anchor); 40 | 41 | 42 | 43 | Value = [ 0 ;0;0; -1 ]; 44 | 45 | j=2; 46 | while ( j<=num_Poses && Value(4) <= 0.5 ) 47 | 48 | tempAss_Anchor = all_PosesNames{j}; 49 | SS =regexp(tempAss_Anchor,'e','split'); 50 | index_Asspose = SS{2}; 51 | index_Asspose = str2num(index_Asspose); 52 | Robot_temp = RobotState{index_Asspose+1}; 53 | pose_temp = Robot_temp.pose; 54 | 55 | tempuvArray{2} = landmark_i_info.(tempAss_Anchor); 56 | 57 | tempValue = GetParallexFromTwoPoses( pose_m, pose_temp, tempuvArray ); 58 | 59 | if tempValue(4)> Value(4) 60 | Value = tempValue; 61 | uvArray = tempuvArray; 62 | Ass_Anchor = tempAss_Anchor; 63 | end 64 | 65 | j = j +1 ; 66 | end 67 | 68 | landmark_i_info.AssAnchor = Ass_Anchor; 69 | landmark_i_info.Value = Value; 70 | 71 | 72 | end -------------------------------------------------------------------------------- /PBAexample/MakeGraphPBA.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = MakeGraphPBA( NewLandmarkManager ) 2 | [ Graph ] = InitializeGraph; 3 | Graph.Fixed.IDname.pose0 = 1; 4 | Graph.Fixed.IDname.pose1 = 1; 5 | 6 | All_landmarks_names = fields(NewLandmarkManager); 7 | num_landmarks = size(All_landmarks_names,1); 8 | 9 | for i = 1: num_landmarks 10 | 11 | thislandmark_name = All_landmarks_names{i}; 12 | thislandmark_info = NewLandmarkManager.(thislandmark_name); 13 | 14 | Graph = MakeGraphForOneLandmark( Graph, thislandmark_info, thislandmark_name); 15 | end 16 | 17 | 18 | end 19 | 20 | 21 | function Graph = MakeGraphForOneLandmark( Graph, thislandmark_info,thislandmark_name) 22 | 23 | isTestEdge =1; 24 | 25 | 26 | 27 | all_PosesNames= fields(thislandmark_info); 28 | num_Poses = size(all_PosesNames,1); 29 | 30 | MainAnchor_name = thislandmark_info.MainAnchor; 31 | AssAnchor_name = thislandmark_info.AssAnchor; 32 | 33 | 34 | for i = 1:num_Poses 35 | 36 | current_posename = all_PosesNames{i}; 37 | uv = thislandmark_info.(current_posename); 38 | Measurement=[]; 39 | Measurement.value.uv = uv ; 40 | Measurement.value.Tcb = [eye(3) zeros(3,1)]; 41 | 42 | if isTestEdge 43 | Measurement.inf = eye(3); 44 | else Measurement.inf = eye(2); 45 | end 46 | 47 | if strcmpi( current_posename, MainAnchor_name) 48 | NodeArray = []; 49 | %NodeArray{1,1} = 'LPose3'; NodeArray{1,2} = MainAnchor_name; 50 | %NodeArray{2,1} = 'LPose3'; NodeArray{2,2} = AssAnchor_name; 51 | NodeArray{1,1} = 'ParallaxPoint'; NodeArray{1,2} = thislandmark_name; 52 | if isTestEdge 53 | [ Graph ] = AddComplexEdge( Graph, 'VisionTestPBA_MainAnchor_Factor', NodeArray, Measurement ); 54 | else 55 | [ Graph ] = AddComplexEdge( Graph, 'VisionPBA_MainAnchor_Factor', NodeArray, Measurement ); 56 | end 57 | elseif strcmpi( current_posename, AssAnchor_name) 58 | NodeArray = []; 59 | NodeArray{1,1} = 'LPose3'; NodeArray{1,2} = MainAnchor_name; 60 | NodeArray{2,1} = 'LPose3'; NodeArray{2,2} = AssAnchor_name; 61 | NodeArray{3,1} = 'ParallaxPoint'; NodeArray{3,2} = thislandmark_name; 62 | if isTestEdge 63 | [ Graph ] = AddComplexEdge( Graph, 'VisionTestPBA_AssAnchor_Factor', NodeArray, Measurement ); 64 | else 65 | [ Graph ] = AddComplexEdge( Graph, 'VisionPBA_AssAnchor_Factor', NodeArray, Measurement ); 66 | end 67 | elseif ~(strcmpi( current_posename, 'Value') || strcmpi( current_posename, 'MainAnchor') || strcmpi( current_posename, 'AssAnchor')) 68 | NodeArray = []; 69 | NodeArray{1,1} = 'LPose3'; NodeArray{1,2} = MainAnchor_name; 70 | NodeArray{2,1} = 'LPose3'; NodeArray{2,2} = AssAnchor_name; 71 | NodeArray{3,1} = 'LPose3'; NodeArray{3,2} = current_posename; 72 | NodeArray{4,1} = 'ParallaxPoint'; NodeArray{4,2} = thislandmark_name; 73 | if isTestEdge 74 | [ Graph ] = AddComplexEdge( Graph, 'VisionTestPBA_Factor', NodeArray, Measurement ); 75 | else 76 | [ Graph ] = AddComplexEdge( Graph, 'VisionPBA_Factor', NodeArray, Measurement ); 77 | end 78 | end 79 | end 80 | 81 | Graph.Nodes.ParallaxPoint.Values.(thislandmark_name) = thislandmark_info.Value; 82 | end 83 | 84 | -------------------------------------------------------------------------------- /PBAexample/PBA_Example.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('../g2o_files/'); 4 | addpath('../auxilliary/') 5 | addpath('../Math/'); 6 | addpath('../Factor/'); 7 | addpath('../Factor/PBA_factor'); 8 | addpath('../Geometry/'); 9 | addpath('../VINS_DataGenerator/'); 10 | 11 | 12 | filename = 'VINS_cubic3.mat'; 13 | num_poses = 49; 14 | tic 15 | NewLandmarkManager = GeneratePBALandmarkManager(filename, num_poses); 16 | 17 | % Set Main and AssAnchor, generate the initial guess for ParallaxPoint 18 | NewLandmarkManager = ... 19 | MakeAnchorAndGenerateInitial( NewLandmarkManager, filename ); 20 | 21 | 22 | [Graph] = MakeGraphPBA(NewLandmarkManager); 23 | 24 | [Graph] = AddInitialGuessForPoses(Graph, filename); 25 | 26 | 27 | 28 | 29 | toc 30 | Graph.Schur.ParallaxPoint = 1; 31 | tic 32 | [ Graph ] = PerformGO_DLnew_sch( Graph ); 33 | toc -------------------------------------------------------------------------------- /PBAexample/VINS3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/PBAexample/VINS3.mat -------------------------------------------------------------------------------- /PBAexample/VINS_cubic3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/PBAexample/VINS_cubic3.mat -------------------------------------------------------------------------------- /PBAexample_2/AddInitialGuessForPoses.m: -------------------------------------------------------------------------------- 1 | function [Graph] = AddInitialGuessForPoses(Graph, filename) 2 | 3 | load (filename); 4 | 5 | all_Pose_name = fields(Graph.Nodes.LPose3.Values); 6 | num_poses = size( all_Pose_name,1); 7 | 8 | for i = 1: num_poses 9 | 10 | thisPose_name = all_Pose_name{i}; 11 | SS =regexp(thisPose_name,'e','split'); 12 | index_pose = SS{2}; 13 | pose_No = str2num( index_pose ); 14 | 15 | 16 | pose = RobotState{pose_No+1}.pose; 17 | 18 | if pose_No~=0 && pose_No~=1 19 | R = pose(1:3,1:3)*expm(skew( randn(3,1)*0.1)); 20 | t = pose(1:3,4)+randn(3,1)*0.1; 21 | pose = [R t]; 22 | end 23 | 24 | Graph.Nodes.LPose3.Values.(thisPose_name)=pose; 25 | 26 | end 27 | 28 | 29 | end 30 | 31 | -------------------------------------------------------------------------------- /PBAexample_2/GeneratePBALandmarkManager.m: -------------------------------------------------------------------------------- 1 | function [ NewLandmarkManager ] = GeneratePBALandmarkManager( filename , num_poses) 2 | 3 | 4 | load (filename); 5 | 6 | 7 | 8 | S_max= find(Measurement_Vision(:,1)== num_poses , 1, 'last' ); 9 | [ LandmarkManager ] = Fn_LandmarkManager( Measurement_Vision(1: S_max, :) ); 10 | 11 | all_landmarkNames = fields(LandmarkManager); 12 | num_landmarks =size(all_landmarkNames,1); 13 | 14 | % delete all landmarks observed only once 15 | NewLandmarkManager = struct; 16 | for i=1:num_landmarks 17 | name_i = all_landmarkNames{i}; 18 | 19 | numOBs_thisLandmark = size(fields(LandmarkManager.(name_i)),1); 20 | 21 | if numOBs_thisLandmark>2 22 | NewLandmarkManager.(name_i) = LandmarkManager.(name_i); 23 | end 24 | end 25 | 26 | 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /PBAexample_2/MakeAnchorAndGenerateInitial.m: -------------------------------------------------------------------------------- 1 | function NewLandmarkManager = MakeAnchorAndGenerateInitial( LandmarkManager, filename ) 2 | 3 | 4 | all_landmarkNames = fields(LandmarkManager); 5 | num_landmarks =size(all_landmarkNames,1); 6 | 7 | 8 | for i = 1: num_landmarks 9 | 10 | name_i = all_landmarkNames{i}; 11 | landmark_i_info = LandmarkManager.(name_i); 12 | Newlandmark_i_info = MakeAnchorAndGenerateInitial_ForEachLandmark(landmark_i_info, filename); 13 | 14 | NewLandmarkManager.(name_i) = Newlandmark_i_info; 15 | 16 | end 17 | 18 | 19 | 20 | end 21 | 22 | function landmark_i_info = MakeAnchorAndGenerateInitial_ForEachLandmark(landmark_i_info, filename) 23 | 24 | load (filename); 25 | 26 | all_PosesNames= fields(landmark_i_info); 27 | num_Poses = size(all_PosesNames,1); 28 | 29 | main_Anchor = all_PosesNames{1}; 30 | landmark_i_info.MainAnchor = main_Anchor; 31 | 32 | 33 | 34 | SS =regexp(main_Anchor,'e','split'); 35 | index_pose = SS{2}; 36 | index_pose = str2num( index_pose ); 37 | pose_m = RobotState{index_pose+1}.pose; 38 | uvArray{1} = landmark_i_info.(main_Anchor); 39 | tempuvArray{1} = landmark_i_info.(main_Anchor); 40 | 41 | 42 | 43 | Value = [ 0 ;0;0; -1 ]; 44 | 45 | j=2; 46 | while ( j<=num_Poses && Value(4) <= 0.5 ) 47 | 48 | tempAss_Anchor = all_PosesNames{j}; 49 | SS =regexp(tempAss_Anchor,'e','split'); 50 | index_Asspose = SS{2}; 51 | index_Asspose = str2num(index_Asspose); 52 | Robot_temp = RobotState{index_Asspose+1}; 53 | pose_temp = Robot_temp.pose; 54 | 55 | tempuvArray{2} = landmark_i_info.(tempAss_Anchor); 56 | 57 | tempValue = GetParallexFromTwoPoses( pose_m, pose_temp, tempuvArray ); 58 | 59 | if tempValue(4)> Value(4) 60 | Value = tempValue; 61 | uvArray = tempuvArray; 62 | Ass_Anchor = tempAss_Anchor; 63 | end 64 | 65 | j = j +1 ; 66 | end 67 | 68 | landmark_i_info.AssAnchor = Ass_Anchor; 69 | landmark_i_info.Value = Value; 70 | 71 | 72 | end -------------------------------------------------------------------------------- /PBAexample_2/MakeGraphPBA.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = MakeGraphPBA( NewLandmarkManager ) 2 | [ Graph ] = InitializeGraph; 3 | Graph.Fixed.IDname.pose0 = 1; 4 | Graph.Fixed.IDname.pose1 = 1; 5 | 6 | All_landmarks_names = fields(NewLandmarkManager); 7 | num_landmarks = size(All_landmarks_names,1); 8 | 9 | for i = 1: num_landmarks 10 | 11 | thislandmark_name = All_landmarks_names{i}; 12 | thislandmark_info = NewLandmarkManager.(thislandmark_name); 13 | 14 | Graph = MakeGraphForOneLandmark( Graph, thislandmark_info, thislandmark_name); 15 | end 16 | 17 | 18 | end 19 | 20 | 21 | function Graph = MakeGraphForOneLandmark( Graph, thislandmark_info,thislandmark_name) 22 | 23 | isTestEdge =1; 24 | 25 | 26 | 27 | all_PosesNames= fields(thislandmark_info); 28 | num_Poses = size(all_PosesNames,1); 29 | 30 | MainAnchor_name = thislandmark_info.MainAnchor; 31 | AssAnchor_name = thislandmark_info.AssAnchor; 32 | 33 | 34 | for i = 1:num_Poses 35 | 36 | current_posename = all_PosesNames{i}; 37 | uv = thislandmark_info.(current_posename); 38 | Measurement=[]; 39 | Measurement.value.uv = uv ; 40 | Measurement.value.Tcb = [eye(3) zeros(3,1)]; 41 | 42 | if isTestEdge 43 | Measurement.inf = eye(3); 44 | else Measurement.inf = eye(2); 45 | end 46 | 47 | if strcmpi( current_posename, MainAnchor_name) 48 | NodeArray = []; 49 | %NodeArray{1,1} = 'LPose3'; NodeArray{1,2} = MainAnchor_name; 50 | %NodeArray{2,1} = 'LPose3'; NodeArray{2,2} = AssAnchor_name; 51 | NodeArray{1,1} = 'ParallaxPoint'; NodeArray{1,2} = thislandmark_name; 52 | if isTestEdge 53 | [ Graph ] = AddComplexEdge( Graph, 'VisionTestPBA_MainAnchor_Factor', NodeArray, Measurement ); 54 | else 55 | [ Graph ] = AddComplexEdge( Graph, 'VisionPBA_MainAnchor_Factor', NodeArray, Measurement ); 56 | end 57 | elseif strcmpi( current_posename, AssAnchor_name) 58 | NodeArray = []; 59 | NodeArray{1,1} = 'LPose3'; NodeArray{1,2} = MainAnchor_name; 60 | NodeArray{2,1} = 'LPose3'; NodeArray{2,2} = AssAnchor_name; 61 | NodeArray{3,1} = 'ParallaxPoint'; NodeArray{3,2} = thislandmark_name; 62 | if isTestEdge 63 | [ Graph ] = AddComplexEdge( Graph, 'VisionTestPBA_AssAnchor_Factor', NodeArray, Measurement ); 64 | else 65 | [ Graph ] = AddComplexEdge( Graph, 'VisionPBA_AssAnchor_Factor', NodeArray, Measurement ); 66 | end 67 | elseif ~(strcmpi( current_posename, 'Value') || strcmpi( current_posename, 'MainAnchor') || strcmpi( current_posename, 'AssAnchor')) 68 | NodeArray = []; 69 | NodeArray{1,1} = 'LPose3'; NodeArray{1,2} = MainAnchor_name; 70 | NodeArray{2,1} = 'LPose3'; NodeArray{2,2} = AssAnchor_name; 71 | NodeArray{3,1} = 'LPose3'; NodeArray{3,2} = current_posename; 72 | NodeArray{4,1} = 'ParallaxPoint'; NodeArray{4,2} = thislandmark_name; 73 | if isTestEdge 74 | [ Graph ] = AddComplexEdge( Graph, 'VisionTestPBA_Factor', NodeArray, Measurement ); 75 | else 76 | [ Graph ] = AddComplexEdge( Graph, 'VisionPBA_Factor', NodeArray, Measurement ); 77 | end 78 | end 79 | end 80 | 81 | Graph.Nodes.ParallaxPoint.Values.(thislandmark_name) = thislandmark_info.Value; 82 | end 83 | 84 | -------------------------------------------------------------------------------- /PBAexample_2/PBA_Example.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('../g2o_files/'); 4 | addpath('../auxilliary/') 5 | addpath('../Math/'); 6 | addpath('../Factor/'); 7 | addpath('../Factor/PBA_factor'); 8 | addpath('../Geometry/'); 9 | addpath('../VINS_DataGenerator/'); 10 | 11 | 12 | pose0=[eye(3) zeros(3,1)]; 13 | pose1=[expm(skew([ 0.1 ;-0.05; 0.2 ])) [ 2; -2 ;-1] ]; 14 | pose2=[expm(skew([ -0.15 ; 0.05; -0.2 ])) [ -1.5; 2.3 ;-1.2] ]; 15 | pose3=[expm(skew([ 0.4 ; -0.1; 0.35 ])) [ -3; -2.7 ;-2] ]; 16 | 17 | % pose1=[eye(3) [ 0; 0 ;1.1] ]; 18 | % pose2=[eye(3) [ 0; 0 ;1.1] ]; 19 | % pose3=[eye(3) [ 0; 0 ;1.1] ]; 20 | 21 | 22 | 23 | RobotState{1}.id = 'pose0'; RobotState{1}.pose = pose0; 24 | RobotState{2}.id = 'pose1'; RobotState{2}.pose = pose1; 25 | RobotState{3}.id = 'pose2'; RobotState{3}.pose = pose2; 26 | RobotState{4}.id = 'pose3'; RobotState{4}.pose = pose3; 27 | 28 | 29 | f=cell(1,10); 30 | f{1} = [ 3; 6 ;9]; 31 | f{2} = [ -1; 2 ;7]; 32 | f{3} = [ -2; 1 ; 11 ]; 33 | f{4} = [ -1.5; 2.4 ; 7.6 ]; 34 | f{5}= [ 3; 2 ; 9.4 ]; 35 | f{6}= [ 0; 5 ; 8 ]; 36 | f{7}= [ 0; 0 ; 100000000 ]; 37 | f{8}= [ 0; 0 ; 9500000000000000 ]; 38 | f{9}= [ 0; 0 ; 4 ]; 39 | f{10}= [ 0.2; 0.5 ; 5 ]; 40 | fprintf('Ground Truth of landmarks\n') 41 | 42 | Measurement_Vision = []; 43 | 44 | for i=1:4 45 | 46 | pose = RobotState{i}.pose; 47 | for j=1:10 48 | landmark = f{j}; 49 | uv = GenerateUV_randn( pose, f{j} )'; 50 | 51 | Measurement_Vision = [Measurement_Vision; i-1 j uv(1) uv(2) ]; 52 | end 53 | end 54 | 55 | save newVINS.mat; 56 | 57 | 58 | filename = 'newVINS.mat'; 59 | num_poses = 3; 60 | tic 61 | NewLandmarkManager = GeneratePBALandmarkManager(filename, num_poses); 62 | 63 | % Set Main and AssAnchor, generate the initial guess for ParallaxPoint 64 | NewLandmarkManager = ... 65 | MakeAnchorAndGenerateInitial( NewLandmarkManager, filename ); 66 | 67 | 68 | [Graph] = MakeGraphPBA(NewLandmarkManager); 69 | 70 | [Graph] = AddInitialGuessForPoses(Graph, filename); 71 | 72 | 73 | Graph.Schur.ParallaxPoint = 1; 74 | 75 | 76 | toc 77 | 78 | [ Graph ] = PerformGO_DLnew( Graph ); 79 | -------------------------------------------------------------------------------- /PBAexample_2/TestVision_Example_Small.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | % addpath('./g2o_files/'); 4 | % addpath('./auxilliary/') 5 | % addpath('./Math/'); 6 | % addpath('./Factor/'); 7 | 8 | 9 | pose0=[eye(3) zeros(3,1)]; 10 | pose1=[expm(skew([ 0.1 ;-0.05; 0.2 ])) [ 2; -2 ;-1] ]; 11 | pose2=[expm(skew([ -0.15 ; 0.05; -0.2 ])) [ -1.5; 2.3 ;-1.2] ]; 12 | pose3=[expm(skew([ 0.4 ; -0.1; 0.35 ])) [ -3; -2.7 ;-2] ]; 13 | 14 | RobotState{1}.id = 'pose0'; RobotState{1}.pose = pose0; 15 | RobotState{2}.id = 'pose1'; RobotState{2}.pose = pose1; 16 | RobotState{3}.id = 'pose2'; RobotState{3}.pose = pose2; 17 | RobotState{4}.id = 'pose3'; RobotState{4}.pose = pose3; 18 | 19 | 20 | f=cell(1,10); 21 | f{1} = [ 1; 2 ;9 ]; 22 | f{2} = [ -1; 2 ;7]; 23 | f{3} = [ -2; 1 ; 11 ]; 24 | f{4} = [ -1.5; 2.4 ; 7.6 ]; 25 | f{5}= [ 3; 2 ; 9.4 ]; 26 | f{6}= [ -4; 4 ; 1400000 ]; 27 | f{7}= [ 4; -6 ; 100000000 ]; 28 | f{8}= [ 5; -2 ; 9.5 ]; 29 | f{9}= [ 0; 0 ; 4 ]; 30 | f{10}= [ 0.2; 0.5 ; 5 ]; 31 | 32 | fprintf('Ground Truth of landmarks\n') 33 | 34 | [ Graph ] = InitializeGraph; 35 | Graph.Fixed.IDname.pose0 = 1; 36 | Graph.Fixed.IDname.pose1 = 1; 37 | 38 | Measurement_Vision = []; 39 | 40 | for i=1:4 41 | 42 | pose = RobotState{i}.pose; 43 | for j=1:10 44 | landmark = f{j}; 45 | uv = GenerateUV_randn( pose, f{j} )'; 46 | 47 | Measurement_Vision = [Measurement_Vision; i-1 j uv(1) uv(2) ]; 48 | end 49 | end 50 | 51 | -------------------------------------------------------------------------------- /PBAexample_2/newVINS.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/PBAexample_2/newVINS.mat -------------------------------------------------------------------------------- /PlotTrajectory.m: -------------------------------------------------------------------------------- 1 | function y = PlotTrajectory( Graph ) 2 | 3 | Landmarks_es = Graph.Nodes.Landmark3.Values; 4 | Robot_es = Graph.Nodes.Pose3.Values; 5 | 6 | PoseArray = fields(Robot_es); 7 | sizePose = size( PoseArray,1 ); 8 | 9 | LandmarkArray = fields(Landmarks_es); 10 | sizeLandmark = size( LandmarkArray,1 ); 11 | 12 | 13 | Position_es =[]; 14 | 15 | F_es = []; 16 | 17 | for i=1:sizePose 18 | pose_i = Robot_es.( PoseArray{i} ); 19 | position_i = pose_i(1:3,4); 20 | 21 | Position_es=[Position_es position_i]; 22 | end 23 | 24 | for i=1:sizeLandmark 25 | f_i = Landmarks_es.( LandmarkArray{i} ); 26 | F_es=[F_es f_i]; 27 | end 28 | 29 | 30 | plot3(Position_es(1,:), Position_es(2,:),Position_es(3,:),'b'); 31 | hold on; 32 | plot3(F_es(1,:), F_es(2,:),F_es(3,:),'go'); 33 | xlabel('X unit,: m'); 34 | ylabel('Y unit,: m'); 35 | zlabel('Z unit,: m'); 36 | 37 | 38 | axis equal 39 | 40 | 41 | 42 | y=0; 43 | 44 | end -------------------------------------------------------------------------------- /PlotTrajectory_Ground.m: -------------------------------------------------------------------------------- 1 | function [ y ] = PlotTrajectory_Ground( RobotState, num_poses ) 2 | 3 | XX=[]; 4 | for i=1:num_poses 5 | 6 | pose = RobotState{i}.pose; 7 | p = pose(1:3,4); 8 | 9 | XX=[XX p]; 10 | 11 | end 12 | 13 | 14 | plot3( XX(1,:), XX(2,:),XX(3,:),'b'); 15 | 16 | title('ground truth'); 17 | xlabel('X unit,: m'); 18 | ylabel('Y unit,: m'); 19 | zlabel('Z unit,: m'); 20 | axis equal 21 | 22 | y=0; 23 | 24 | end 25 | 26 | -------------------------------------------------------------------------------- /PoseFixed_NewExample_VINS_visual.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | addpath('./Geometry/'); 8 | addpath('./VINS_DataGenerator/'); 9 | 10 | load VINS_cubic3.mat 11 | 12 | [ Graph ] = InitializeGraph; 13 | Graph.Fixed.IDname.pose0=1; % fix the variable pose0 14 | Graph.Fixed.IDname.pose1=1; % fix the variable pose1 15 | 16 | Graph.Schur.Landmark3 = 1; % perform schur docomposition for the Nodetype Landmark3 17 | 18 | 19 | [ Graph2 ] = InitializeGraph; 20 | 21 | 22 | num_poses = 49; 23 | 24 | ss=1; 25 | while( Measurement_Vision(ss,1)<= num_poses) 26 | 27 | ss = ss+1; 28 | 29 | end 30 | 31 | i=1; 32 | 33 | 34 | 35 | while( Measurement_Vision(i,1)<= num_poses) 36 | NodeArray=[]; 37 | pose_id = ['pose' num2str( Measurement_Vision(i,1) )]; 38 | pose_index = Measurement_Vision(i,1); 39 | landmark_id =['landmark' num2str( Measurement_Vision(i,2) )]; 40 | 41 | 42 | 43 | if isfield(Graph.Nodes, 'Landmark3' ) 44 | if isfield(Graph.Nodes.Landmark3.Values, landmark_id) 45 | kk =1 ; 46 | else kk=0; 47 | end 48 | else kk=0; 49 | end 50 | num_landmark_id = Measurement_Vision(i,2); 51 | num_ob_id = sum(Measurement_Vision(1:ss-1,2)==num_landmark_id); 52 | 53 | 54 | if num_ob_id>=2 55 | NodeArray{1,1}='Pose3'; NodeArray{1,2}=pose_id; 56 | NodeArray{2,1}='Landmark3'; NodeArray{2,2}=landmark_id; 57 | Measurement.value= (Measurement_Vision(i,3:4))' ; 58 | Measurement.inf=eye(3); 59 | [ Graph ]= AddComplexEdge(Graph, 'VisionTest_Factor', NodeArray, Measurement); 60 | 61 | Measurement2.value= (Measurement_Vision(i,3:4))' ; 62 | Measurement2.inf=eye(2); 63 | [ Graph2 ]= AddComplexEdge(Graph2, 'Vision_Factor', NodeArray, Measurement2); 64 | if kk == 1 65 | Graph.Nodes.Landmark3.Values.(landmark_id )= Landmarks.(landmark_id)+1.3*randn(3,1); 66 | end 67 | pose = RobotState{pose_index+1}.pose; 68 | R = pose(1:3,1:3)*expm(skew( randn(3,1)*0.3)); 69 | t = pose(1:3,4)+randn(3,1)*0.5; 70 | Graph.Nodes.Pose3.Values.(pose_id)=[ R t ]; 71 | Graph2.Nodes.Pose3.Values.(pose_id)=[ R t ]; 72 | end 73 | 74 | %end 75 | 76 | 77 | i=i+1; 78 | end 79 | 80 | S_max= find(Measurement_Vision(:,1)== num_poses , 1, 'last' ); 81 | [ LandmarkManager ] = Fn_LandmarkManager( Measurement_Vision(1: S_max, :) ); 82 | 83 | PoseValue = Graph.Nodes.Pose3.Values; 84 | PoseValue.pose0 = RobotState{1}.pose; 85 | PoseValue.pose1 = RobotState{2}.pose; 86 | 87 | Graph.Nodes.Pose3.Values.pose0 = RobotState{1}.pose; 88 | Graph.Nodes.Pose3.Values.pose1 = RobotState{2}.pose; 89 | 90 | Graph2.Nodes.Pose3.Values.pose0 = RobotState{1}.pose; 91 | Graph2.Nodes.Pose3.Values.pose1 = RobotState{2}.pose; 92 | 93 | [ Landmarks_values ] = Cal_triangulate( PoseValue , LandmarkManager ); 94 | Graph.Nodes.Landmark3.Values=Landmarks_values; 95 | Graph2.Nodes.Landmark3.Values=Landmarks_values; 96 | 97 | 98 | 99 | subplot(1,2,1); 100 | PlotTrajectory( Graph ); 101 | title('Initial Guess'); 102 | 103 | 104 | tic 105 | [ Graph ] = PerformGO_DLnew_sch( Graph ); 106 | toc 107 | 108 | subplot(1,2,2) 109 | PlotTrajectory( Graph ); 110 | title('After Optimization'); 111 | axis([-5 5 -5 5 -5 5]*8/5) 112 | 113 | %fprintf('Graph2 Begins ...'); 114 | 115 | 116 | %Graph2.Nodes.Pose3.Values = Graph.Nodes.Pose3.Values; 117 | %Graph2.Nodes.Landmark3.Values = Graph.Nodes.Landmark3.Values; 118 | %[ Graph2 ] = PerformGO_LM( Graph2 ); 119 | 120 | 121 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This is a matlab code package for **nonlinear least squares optimization**, based on the well-known concept--**Factor Graph**. 2 | 3 | # Organization 4 | 1. Data: store the data to be processed 5 | 2. Factor: the edge and node 6 | 3. g2o_files: core, provide the main framework of the nonlinear least squares 7 | 4. Math: provide the mathematical operation like so3_exp,... 8 | 5. auxilliary: others 9 | 6. Geometry: some operations on geometry such as triangulation 10 | 7. Doc: two tutorial notes 11 | 12 | # Document 13 | 1. A tutorial for Optimization on Manifold 14 | 2. A tutorial for Graph Optimization 15 | 16 | # Customization 17 | This code allows users to define new variable nodes and new factors/edges/cost functions. 18 | The framework is reorganized with necessary warnings for the extension of the new node and new edge. 19 | 20 | 1. When the new node is defined, the information needs to be given in the “GetNodeTypeDimension”, “SetNodeDefaultValue” and “update_state”. 21 | 2. When the new edge is defined, the information needs to be given in “GetFactorX_format” and “GetEdgeTypeDimension”. 22 | 23 | # Examples for study/use 24 | 1. When you want to perform the estimation for 2D RGBD case, just run “Example_VictoriaPark.m”. 25 | 2. When you want to perform the estimation for 3D vision case, just run “Vision_Example_Small.m”. 26 | 27 | 28 | # Updates 29 | 1. Any variable can be fix in the process of optimization 30 | 2. Schur decomposition has been added 31 | 3. Levenberg-Marquart and Powell's Dogleg have been added 32 | 4. A novel IMU factor for VINS has been added 33 | 5. Parallax vision factors have been added 34 | 6. VisionTest fator has been added 35 | -------------------------------------------------------------------------------- /SecondExample_VINS_visual.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | addpath('./VINS_DataGenerator/'); 8 | 9 | load VINS_cubic3.mat 10 | 11 | [ Graph ] = InitializeGraph; 12 | 13 | 14 | %%% add Prior 15 | M_initial.value = RobotState{1, 1}.pose; 16 | M_initial.inf=100*eye(6); 17 | [ Graph ] = AddUnaryEdge( Graph, 'PriorPose3_Factor', 'Pose3', 'pose0', M_initial ); 18 | Graph.Nodes.Pose3.Values.pose0 = RobotState{1, 1}.pose; 19 | 20 | Measurement.value.vel = RobotState{1, 1}.velocity ; 21 | Measurement.value.bias = [RobotState{1, 1}.ba; RobotState{1, 1}.bw]; 22 | Measurement.inf=100*eye(9); 23 | NodeArray=cell(2,2); 24 | NodeArray{1,1}='Velocity3'; NodeArray{1,2}='v0'; 25 | NodeArray{2,1}='IMUbias'; NodeArray{2,2}='b0'; 26 | [ Graph ]= AddComplexEdge(Graph, 'PriorVelAndbias_Factor', NodeArray, Measurement); 27 | Graph.Nodes.Velocity3.Values.v0 = RobotState{1, 1}.velocity; 28 | Graph.Nodes.IMUbias.Values.b0 = Measurement.value.bias; 29 | 30 | 31 | % 32 | Measure_translation.inf=10000*eye(6); 33 | T0= RobotState{1, 1}.pose; R0=T0(1:3,1:3); p0=T0(1:3,4); 34 | T1= RobotState{1, 2}.pose; R1=T1(1:3,1:3); p1=T1(1:3,4); 35 | 36 | 37 | Measure_translation.value=[R0'*R1 R0'*(p1-p0) ]; 38 | 39 | [ Graph ] = AddNormalEdge( Graph, 'RelativePose3_Factor', 'Pose3', 'pose0', 'Pose3', 'pose1', Measure_translation ); 40 | 41 | 42 | 43 | % 44 | 45 | %%% add IMU measurement 46 | num_poses =10; 47 | 48 | for i = 0: num_poses-1 49 | % %%%%%%%% IMU part 50 | % NodeArray{1,1}='Pose3'; NodeArray{1,2}=['pose' num2str(i)]; 51 | % NodeArray{2,1}='Velocity3'; NodeArray{2,2}=['v' num2str(i)]; 52 | % NodeArray{3,1}='IMUbias'; NodeArray{3,2}=['b' num2str(i)]; 53 | % NodeArray{4,1}='Pose3'; NodeArray{4,2}=['pose' num2str(i+1)]; 54 | % NodeArray{5,1}='Velocity3'; NodeArray{5,2}=['v' num2str(i+1)]; 55 | % NodeArray{6,1}='IMUbias'; NodeArray{6,2}=['b' num2str(i+1)]; 56 | % Measurement_IMU_i = Measurement_IMU{i+1}; 57 | % [ Graph ]= AddComplexEdge(Graph, 'IMU_Factor', NodeArray, Measurement_IMU_i); 58 | % 59 | % 60 | % NodeArray_bias{1,1}='IMUbias'; NodeArray_bias{1,2}=['b' num2str(i)]; 61 | % NodeArray_bias{2,1}='IMUbias'; NodeArray_bias{2,2}=['b' num2str(i+1)]; 62 | % Measurement_IMU_change.value = []; 63 | % Measurement_IMU_change.inf = IMUbiasWalk_inf; 64 | % [ Graph ]= AddComplexEdge(Graph, 'IMUbias_Factor', NodeArray_bias, Measurement_IMU_change); 65 | % 66 | % if 1 67 | Graph.Nodes.Pose3.Values.(NodeArray{1,2})=RobotState{i+1}.pose; 68 | % Graph.Nodes.Pose3.Values.(NodeArray{4,2})=RobotState{i+2}.pose; 69 | % Graph.Nodes.Velocity3.Values.(NodeArray{2,2})= RobotState{i+1}.velocity; 70 | % Graph.Nodes.Velocity3.Values.(NodeArray{5,2})= RobotState{i+2}.velocity; 71 | % Graph.Nodes.IMUbias.Values.(NodeArray{3,2})= [RobotState{i+1}.ba;RobotState{i+1}.bw]; 72 | % Graph.Nodes.IMUbias.Values.(NodeArray{6,2})= [RobotState{i+2}.ba;RobotState{i+2}.bw]; 73 | % end 74 | %[ Graph ] = PerformGO( Graph ); % To get odometry 75 | 76 | 77 | for j=1:50 78 | landmark_id =['landmark' num2str(j)]; 79 | pose_id = ['pose' num2str(i)]; 80 | Measurement.value= (Measurement_Vision( j+ 50*i ,3:4))' ; 81 | depth_rough = norm(RGBD_data(j+ 50*i,3:5))*(1+randn*0.1); 82 | 83 | %Measurement.inf= eye(2)*(1/depth_rough^2)*(1^2) ; 84 | Measurement.inf= eye(3) ; 85 | NodeArrayA{1,1}='Pose3'; NodeArrayA{1,2}=pose_id; 86 | NodeArrayA{2,1}='Landmark3'; NodeArrayA{2,2}=landmark_id; 87 | [ Graph ]= AddComplexEdge(Graph, 'VisionTest_Factor', NodeArrayA, Measurement); 88 | if i <=2 89 | Graph.Nodes.Landmark3.Values.(landmark_id )= Landmarks.(landmark_id)+randn(3,1)*0.3; 90 | end 91 | end 92 | 93 | % if i >2 94 | % [ Graph ] = PerformGO( Graph ); 95 | % end 96 | 97 | end 98 | [ Graph ] = PerformGO_DL( Graph ); 99 | 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /TestVision_Example_Small.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | addpath('./Geometry/'); 8 | 9 | 10 | 11 | pose0=[eye(3) zeros(3,1)]; 12 | pose1=[expm(skew([ 0.1 ;-0.05; 0.2 ])) [ 2; -2 ;-1] ]; 13 | pose2=[expm(skew([ -0.15 ; 0.05; -0.2 ])) [ -1.5; 2.3 ;-1.2] ]; 14 | pose3=[expm(skew([ 0.4 ; -0.1; 0.35 ])) [ -3; -2.7 ;-2] ]; 15 | 16 | 17 | f=cell(1,10); 18 | f{1} = [ 1; 2 ;4 ]; 19 | f{2} = [ -1; 2 ;7]; 20 | f{3} = [ -2; 1 ; 11 ]; 21 | f{4} = [ -1.5; 2.4 ; 7.6 ]; 22 | f{5}= [ 3; 2 ; 9.4 ]; 23 | f{6}= [ -4; 4 ; 3 ]; 24 | f{7}= [ 4; -6 ; 5 ]; 25 | f{8}= [ 5; -2 ; 9.5 ]; 26 | f{9}= [ 0; 0 ; 4 ]; 27 | f{10}= [ 0.2; 0.5 ; 5 ]; 28 | 29 | fprintf('Ground Truth of landmarks\n') 30 | 31 | %%%%% co-planar 32 | % f{1} = [ 1; 2 ;5 ]; 33 | % f{2} = [ -1; 2 ;5]; 34 | % f{3} = [ -2; 1 ; 5 ]; 35 | % f{4} = [ -1.5; 2.4 ; 5 ]; 36 | % f{5}= [ 3; 2 ; 5 ]; 37 | % f{6}= [ -4; 4 ; 5 ]; 38 | % f{7}= [ 4; -6 ; 5 ]; 39 | % f{8}= [ 5; -2 ; 5]; 40 | % f{9}= [ 0; 0 ; 5 ]; 41 | % f{10}= [ 0.2; 0.5 ; 5 ]; 42 | 43 | 44 | 45 | [ Graph ] = InitializeGraph; 46 | Graph.Fixed.IDname.pose0 = 1; 47 | Graph.Fixed.IDname.pose1 = 1; 48 | Graph.Fixed.IDname.pose2 = 1; 49 | Graph.Fixed.IDname.pose3 = 1; 50 | 51 | LandmarkManager=[]; 52 | 53 | k=1; 54 | for i=1:10 55 | fea = f{i}; 56 | landmark_id = ['landmark' num2str(i)]; 57 | [ UV_i_0 ] = GenerateUV_randn( pose0, f{i} ); 58 | 59 | 60 | R = pose0(1:3,1:3); p = pose0(1:3,4); d = norm(R'*( fea - p )); 61 | Measurement_i_0.value = UV_i_0; 62 | NodeArray=cell(2,2); 63 | NodeArray{1,1}='Pose3';NodeArray{1,2}='pose0'; 64 | NodeArray{2,1}='Landmark3';NodeArray{2,2}=['landmark' num2str(i)]; 65 | 66 | 67 | LandmarkManager.(landmark_id).pose0 = UV_i_0; 68 | if k 69 | Measurement_i_0.inf = eye(3); %eye(2)/(d^2)*525^2; 70 | [ Graph ] = AddComplexEdge( Graph, 'VisionTest_Factor', NodeArray, Measurement_i_0 ); 71 | else 72 | Measurement_i_0.inf = eye(2); 73 | [ Graph ] = AddComplexEdge( Graph, 'Vision_Factor', NodeArray, Measurement_i_0 ); 74 | end 75 | 76 | [ UV_i_1 ] = GenerateUV_randn( pose1, f{i} ); 77 | LandmarkManager.(landmark_id).pose1 = UV_i_1; 78 | 79 | R = pose1(1:3,1:3); p = pose1(1:3,4); d = norm(R'*( fea - p )); 80 | Measurement_i_1.value = UV_i_1; 81 | Measurement_i_1.inf = eye(2); 82 | NodeArray{1,2}='pose1'; 83 | if k 84 | Measurement_i_1.inf = eye(3); %eye(2)/(d^2)*525^2; 85 | [ Graph ] = AddComplexEdge( Graph, 'VisionTest_Factor', NodeArray, Measurement_i_1 ); 86 | else 87 | Measurement_i_1.inf = eye(2); 88 | [ Graph ] = AddComplexEdge( Graph, 'Vision_Factor', NodeArray, Measurement_i_1 ); 89 | end 90 | 91 | [ UV_i_2 ] = GenerateUV_randn( pose2, f{i} ); 92 | LandmarkManager.(landmark_id).pose2 = UV_i_2; 93 | R = pose2(1:3,1:3); p = pose2(1:3,4); d = norm(R'*( fea - p )); 94 | Measurement_i_2.value = UV_i_2; 95 | Measurement_i_2.inf = eye(1); 96 | NodeArray{1,2}='pose2'; 97 | if k 98 | Measurement_i_2.inf = eye(3); %eye(2)/(d^2)*525^2; 99 | [ Graph ] = AddComplexEdge( Graph, 'VisionTest_Factor', NodeArray, Measurement_i_2 ); 100 | else 101 | Measurement_i_2.inf = eye(2); 102 | [ Graph ] = AddComplexEdge( Graph, 'Vision_Factor', NodeArray, Measurement_i_2 ); 103 | end 104 | 105 | [ UV_i_3 ] = GenerateUV_randn( pose3, f{i} ); 106 | LandmarkManager.(landmark_id).pose3 = UV_i_3; 107 | R = pose3(1:3,1:3); p = pose3(1:3,4); d = norm(R'*( fea - p )); 108 | Measurement_i_3.value = UV_i_3; 109 | Measurement_i_3.inf = eye(1); 110 | NodeArray{1,2}='pose3'; 111 | if k 112 | Measurement_i_3.inf = eye(3); % eye(2)/(d^2)*525^2; 113 | [ Graph ] = AddComplexEdge( Graph, 'VisionTest_Factor', NodeArray, Measurement_i_3 ); 114 | else 115 | Measurement_i_3.inf = eye(2); 116 | [ Graph ] = AddComplexEdge( Graph, 'Vision_Factor', NodeArray, Measurement_i_3 ); 117 | end 118 | 119 | end 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | %%% Set initial guess via ground truth+noise 128 | Graph.Nodes.Pose3.Values.pose0=pose0; 129 | noise1 = [ expm(skew(randn(3,1)*0)) randn(3,1)*0]; 130 | Graph.Nodes.Pose3.Values.pose1=se3_group(pose1, noise1 ) ; 131 | noise2 = [ expm(skew(rand(3,1)*0)) randn(3,1)*0]; 132 | Graph.Nodes.Pose3.Values.pose2=se3_group(pose2, noise2 ) ; 133 | noise3 = [ expm(skew(randn(3,1)*0)) randn(3,1)*.0]; 134 | Graph.Nodes.Pose3.Values.pose3=se3_group(pose3, noise3 ) ; 135 | 136 | PoseValue.pose0 = pose0; 137 | PoseValue.pose1 = pose1; 138 | PoseValue.pose2 = pose2; 139 | PoseValue.pose3 = pose3; 140 | 141 | Y=cell(10,1); 142 | for i=1:10 143 | G_i=f{i}; 144 | Graph.Nodes.Landmark3.Values.(['landmark' num2str(i)])=f{i}+12*rand(3,1); 145 | %Graph.Nodes.Landmark3.Values.(['landmark' num2str(i)])=[-10 ; -10 ; -10]; 146 | X_i=Graph.Nodes.Landmark3.Values.(['landmark' num2str(i)]); 147 | Y{i}=X_i; 148 | end 149 | %%% Set initial guess 150 | 151 | [ Landmarks_values ] = Cal_triangulate( PoseValue , LandmarkManager ); 152 | Graph.Nodes.Landmark3.Values=Landmarks_values; 153 | 154 | 155 | 156 | %[ Graph ] = PerformGO( Graph ); 157 | tic 158 | [ Graph ] = PerformGO_DLnew_sch( Graph ); 159 | toc 160 | 161 | 162 | for i=1:10 163 | G_i=f{i}; 164 | X_i=Graph.Nodes.Landmark3.Values.(['landmark' num2str(i)]); 165 | Y_i=Y{i}; 166 | fprintf('%d, %f, %f, %f \n', i, G_i(1),X_i(1),Y_i(1) ) 167 | fprintf('%d, %f, %f, %f \n', i, G_i(2),X_i(2),Y_i(2) ) 168 | fprintf('%d, %f, %f, %f \n', i, G_i(3),X_i(3),Y_i(3) ) 169 | end 170 | 171 | 172 | -------------------------------------------------------------------------------- /Third_Example_VINS_visual.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | addpath('./VINS_DataGenerator/'); 8 | addpath('./Geometry/'); 9 | 10 | load VINS4.mat 11 | 12 | [ Graph ] = InitializeGraph; 13 | 14 | %%% add Prior 15 | M_initial.value = RobotState{1, 1}.pose; 16 | M_initial.inf=100*eye(6); 17 | [ Graph ] = AddUnaryEdge( Graph, 'PriorPose3_Factor', 'Pose3', 'pose0', M_initial ); 18 | Graph.Nodes.Pose3.Values.pose0 = RobotState{1, 1}.pose; 19 | 20 | Measurement.value.vel = RobotState{1, 1}.velocity ; 21 | Measurement.value.bias = [RobotState{1, 1}.ba; RobotState{1, 1}.bw]; 22 | Measurement.inf=100*eye(9); 23 | NodeArray=cell(2,2); 24 | NodeArray{1,1}='Velocity3'; NodeArray{1,2}='v0'; 25 | NodeArray{2,1}='IMUbias'; NodeArray{2,2}='b0'; 26 | [ Graph ]= AddComplexEdge(Graph, 'PriorVelAndbias_Factor', NodeArray, Measurement); 27 | Graph.Nodes.Velocity3.Values.v0 = RobotState{1, 1}.velocity; 28 | Graph.Nodes.IMUbias.Values.b0 = Measurement.value.bias; 29 | 30 | 31 | % 32 | Measure_translation.inf=10000*eye(6); 33 | T0= RobotState{1, 1}.pose; R0=T0(1:3,1:3); p0=T0(1:3,4); 34 | T1= RobotState{1, 2}.pose; R1=T1(1:3,1:3); p1=T1(1:3,4); 35 | 36 | 37 | Measure_translation.value=[R0'*R1 R0'*(p1-p0) ]; 38 | 39 | %[ Graph ] = AddNormalEdge( Graph, 'RelativePose3_Factor', 'Pose3', 'pose0', 'Pose3', 'pose1', Measure_translation ); 40 | 41 | 42 | 43 | % 44 | 45 | %%% add IMU measurement 46 | num_poses = 30; 47 | 48 | for i = 0: num_poses-1 49 | %%%%%%%% IMU part 50 | NodeArray=cell(2,6); 51 | NodeArray{1,1}='Pose3'; NodeArray{1,2}=['pose' num2str(i)]; 52 | NodeArray{2,1}='Velocity3'; NodeArray{2,2}=['v' num2str(i)]; 53 | NodeArray{3,1}='IMUbias'; NodeArray{3,2}=['b' num2str(i)]; 54 | NodeArray{4,1}='Pose3'; NodeArray{4,2}=['pose' num2str(i+1)]; 55 | NodeArray{5,1}='Velocity3'; NodeArray{5,2}=['v' num2str(i+1)]; 56 | NodeArray{6,1}='IMUbias'; NodeArray{6,2}=['b' num2str(i+1)]; 57 | Measurement_IMU_i = Measurement_IMU{i+1}; 58 | 59 | [ Graph ]= AddComplexEdge(Graph, 'IMU_Factor', NodeArray, Measurement_IMU_i); 60 | 61 | NodeArray_bias{1,1}='IMUbias'; NodeArray_bias{1,2}=['b' num2str(i)]; 62 | NodeArray_bias{2,1}='IMUbias'; NodeArray_bias{2,2}=['b' num2str(i+1)]; 63 | Measurement_IMU_change.value = []; 64 | Measurement_IMU_change.inf = IMUbiasWalk_inf; 65 | [ Graph ]= AddComplexEdge(Graph, 'IMUbias_Factor', NodeArray_bias, Measurement_IMU_change); 66 | 67 | Graph.Nodes.Pose3.Values.(NodeArray{1,2})=RobotState{i+1}.pose; 68 | Graph.Nodes.Pose3.Values.(NodeArray{4,2})=RobotState{i+2}.pose; 69 | 70 | Graph.Nodes.Velocity3.Values.(NodeArray{2,2})= RobotState{i+1}.velocity; 71 | Graph.Nodes.Velocity3.Values.(NodeArray{5,2})= RobotState{i+2}.velocity; 72 | Graph.Nodes.IMUbias.Values.(NodeArray{3,2})= [RobotState{i+1}.ba;RobotState{i+1}.bw]; 73 | Graph.Nodes.IMUbias.Values.(NodeArray{6,2})= [RobotState{i+2}.ba;RobotState{i+2}.bw]; 74 | 75 | %[ Graph ] = PerformGO( Graph ); % To get odometry 76 | 77 | end 78 | [ Graph ] = PerformGO( Graph ); 79 | 80 | Measurement_Vision_small = Measurement_Vision(1: (1+num_poses)*50,: ); 81 | [ LandmarkManager ] = Fn_LandmarkManager( Measurement_Vision_small ); 82 | Landmarks_values= Cal_triangulate( Graph.Nodes.Pose3.Values , LandmarkManager ); 83 | % %%% add Vision measurement 84 | i=1; 85 | 86 | while( Measurement_Vision(i,1)<= num_poses) 87 | NodeArray=[]; 88 | pose_id = ['pose' num2str( Measurement_Vision(i,1) )]; 89 | pose_index = Measurement_Vision(i,1); 90 | landmark_id =['landmark' num2str( Measurement_Vision(i,2) )]; 91 | j = Measurement_Vision(i,2); 92 | Measurement.value.uv = (Measurement_Vision(i,3:4))' ; 93 | Measurement.value.f = Landmarks_values.(landmark_id); 94 | Measurement.value.id = landmark_id; 95 | depth = norm(RGBD_data(i,3:5))*(1+randn*0.1); 96 | Measurement.inf=eye(2)*(1/depth^2)*525^2; 97 | NodeArray{1,1}='Pose3'; NodeArray{1,2}=pose_id; 98 | [ Graph ]= AddComplexEdge(Graph, 'LinearVisionNoLandmark_Factor', NodeArray, Measurement); 99 | 100 | i=i+1; 101 | end 102 | [ Graph ] = PerformGO( Graph ); 103 | 104 | iter_max = 100; 105 | Graph.Parameters.iter=1; 106 | 107 | for i =1 : iter_max 108 | num_edges = size(Graph.Edges,2); 109 | 110 | for k = 1:num_edges 111 | if strcmp(Graph.Edges{k}.EdgeType, 'LinearVisionNoLandmark_Factor') 112 | landmark_id = Graph.Edges{k}.Measurement.value.id; 113 | Graph.Edges{k}.Measurement.value.f = Landmarks_values.(landmark_id); 114 | end 115 | end 116 | [ Graph ] = PerformGO( Graph ); 117 | Landmarks_values= Cal_triangulate( Graph.Nodes.Pose3.Values , LandmarkManager ); 118 | end 119 | 120 | 121 | 122 | 123 | 124 | -------------------------------------------------------------------------------- /VINS2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS2.mat -------------------------------------------------------------------------------- /VINS3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS3.mat -------------------------------------------------------------------------------- /VINS4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS4.mat -------------------------------------------------------------------------------- /VINS5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS5.mat -------------------------------------------------------------------------------- /VINS_DataGenerator/A_ProduceData/Fun_IMUdataIJ.m: -------------------------------------------------------------------------------- 1 | function [ IMU_DataIJ ] = Fun_IMUdataIJ( i,j, Camera_timestep, IMU ) 2 | 3 | % i means pose i from 0 to Camera.num-1 4 | % i << j strictly 5 | 6 | %Time_start=Time.start; 7 | 8 | % ti=Fun_Camera_step2time( i, Time_start, Camera_timestep ); 9 | % tj=Fun_Camera_step2time( j, Time_start, Camera_timestep ); 10 | 11 | Iindex=floor(i*Camera_timestep/IMU.timestep)+1; 12 | Jindex=floor(j*Camera_timestep/IMU.timestep); 13 | 14 | % if Jindex> size(IMU.gyro.Data,2) 15 | % Jindex=size(IMU.gyro.Data,2); 16 | % end 17 | 18 | 19 | IMU_DataIJ.gyro=IMU.gyro.Data(1:3, Iindex:Jindex ); 20 | IMU_DataIJ.acc=IMU.acc.Data(1:3, Iindex:Jindex ); 21 | IMU_DataIJ.timestep=IMU.timestep; 22 | end 23 | 24 | 25 | % function [ time ] = Fun_Camera_step2time( pointer, Time_start, Camera_timestep ) 26 | % 27 | % time= pointer*Camera_timestep+Time_start; 28 | % 29 | % end 30 | 31 | -------------------------------------------------------------------------------- /VINS_DataGenerator/A_ProduceData/Fun_PreIntegration_bias.m: -------------------------------------------------------------------------------- 1 | function [ value, inf ] = Fun_PreIntegration_bias( IMU_DataIJ, IMUsettings, accbias_es, gyrobias_es ) 2 | 3 | addpath('MathOperation/'); 4 | 5 | 6 | N=blkdiag( IMUsettings.gyro_noise_cov, IMUsettings.acc_noise_cov, zeros(3,3) ); 7 | 8 | 9 | 10 | [ value, inf ] = rk4_manifold_mod( IMU_DataIJ, N, accbias_es, gyrobias_es ); 11 | 12 | 13 | NN = size(IMU_DataIJ.gyro,2); 14 | 15 | 16 | %J(1:3,1:3)=StrictlyRotationMatrix(J(1:3,1:3)); 17 | 18 | 19 | end 20 | 21 | 22 | function R=StrictlyRotationMatrix(M) 23 | 24 | c1=M(1:3,1); 25 | c2=M(1:3,2); 26 | 27 | c1=c1/norm(c1); 28 | c2=c2-(c2'*c1)*c1; 29 | c2=c2/norm(c2); 30 | c3=skew(c1)*c2; 31 | 32 | R=[c1 c2 c3]; 33 | 34 | 35 | end 36 | 37 | -------------------------------------------------------------------------------- /VINS_DataGenerator/A_ProduceData/Fun_generateFeatures.m: -------------------------------------------------------------------------------- 1 | function [ Feature ] = Fun_generateFeatures 2 | 3 | Feature=struct; 4 | Feature.num=50; 5 | 6 | 7 | %radius=10; 8 | %Feature.position=(rand(3,Feature.num)-0.5*ones(3,Feature.num))*2*radius; 9 | 10 | Feature.position=[]; 11 | for i=-2:1:2 12 | for j=-2:1:2 13 | for k=-2:1:2 14 | Feature.position=[Feature.position [i;j;k]]; 15 | end 16 | end 17 | end 18 | 19 | 20 | Feature.num = size(Feature.position ,2 ); 21 | 22 | end 23 | 24 | -------------------------------------------------------------------------------- /VINS_DataGenerator/A_ProduceData/Fun_getData.m: -------------------------------------------------------------------------------- 1 | function [ IMU,Camera,NewVehicle, RGBD_data ] = Fun_getData( Time,IMU,Camera,Feature ) 2 | 3 | NewVehicle=struct; 4 | NewVehicle.orientation=[]; 5 | NewVehicle.position=[]; 6 | NewVehicle.localvelocity=[]; 7 | NewVehicle.globalvelocity=[]; 8 | NewVehicle.gyro_bias_history=IMU.gyro.bias; 9 | NewVehicle.acc_bias_history=IMU.acc.bias; 10 | 11 | loadtime_IMU=Time.start; 12 | loadtime_Camera=Time.start; 13 | 14 | RGBD_data=[]; 15 | 16 | 17 | %while(loadtime_IMU<=Time.end) 18 | while((loadtime_IMU-Time.end)<=0.0000001) 19 | [ Vehicle ] = SimulatorVehicle( loadtime_IMU ); 20 | %%%%% Record IMU data %%%%%% 21 | gyro_covs2= IMU.gyro.noise_cov^(1/2); 22 | IMU.gyro.Data= [IMU.gyro.Data Vehicle.av+gyro_covs2*randn(3,1)/sqrt(IMU.timestep)+IMU.gyro.bias ]; 23 | acc_covs2= IMU.acc.noise_cov^(1/2); 24 | IMU.acc.Data= [IMU.acc.Data Vehicle.accLocal+acc_covs2*randn(3,1)/sqrt(IMU.timestep)+IMU.acc.bias ]; 25 | %%%%%% Record IMU data %%%%%%% 26 | %%%%% Record IMU biases and up these %%%%%%% 27 | IMU.gyro.bias=IMU.gyro.bias+IMU.gyro.bias_sigma*sqrt(IMU.timestep)*randn(3,1); 28 | IMU.acc.bias=IMU.acc.bias+IMU.acc.bias_sigma*sqrt(IMU.timestep)*randn(3,1); 29 | NewVehicle.gyro_bias_history=[NewVehicle.gyro_bias_history IMU.gyro.bias]; 30 | NewVehicle.acc_bias_history=[NewVehicle.acc_bias_history IMU.acc.bias]; 31 | %%%%% Record IMU biases and up these %%%%%%% 32 | %%%%% Record Vehicle Orientation, Position, Velocity_Global, Velocity_Local %%%%%%% 33 | NewVehicle.orientation=[NewVehicle.orientation Vehicle.orientation]; 34 | NewVehicle.position=[NewVehicle.position Vehicle.position]; 35 | NewVehicle.localvelocity=[NewVehicle.localvelocity Vehicle.lv]; 36 | NewVehicle.globalvelocity=[NewVehicle.globalvelocity Vehicle.dp]; 37 | %%%%% Record IMU biases and up these %%%%%%% 38 | IMU.timehistory=[IMU.timehistory loadtime_IMU]; 39 | loadtime_IMU=loadtime_IMU+IMU.timestep; 40 | end 41 | 42 | 43 | 44 | pointer=0; 45 | while((loadtime_Camera-Time.end)<=0.00000001) 46 | [ Vehicle ] = SimulatorVehicle( loadtime_Camera ); 47 | temp=Vehicle.orientation'*(Feature.position-repmat(Vehicle.position,1,Feature.num)); 48 | Dxz=temp(1,:)./temp(3,:); 49 | Dyz=temp(2,:)./temp(3,:); 50 | 51 | 52 | Dxydz= [Dxz;Dyz]; 53 | fx = 525.0; 54 | fy = 525.0; 55 | cx0 = 639.5; 56 | cy0 = 479.5; 57 | K = [ fx 0; 0 fy]; 58 | UV = K*Dxydz+ repmat([cx0;cy0], 1, Feature.num ) +randn(2, Feature.num ) ; 59 | 60 | 61 | 62 | A1=ones( Feature.num,1)*pointer; 63 | A2=1:1:Feature.num; A2=A2'; 64 | A3 = ( UV(1,:) )'; %A3=Dxz'; 65 | A4= ( UV(2,:) )'; %Dyz'; 66 | localDataMatrix=[A1 A2 A3 A4]; 67 | 68 | temp_noise = temp+ 0.1*randn(3, Feature.num); 69 | 70 | localRGBD=[A1 A2 temp_noise']; 71 | 72 | 73 | %%%%% Pick out unexisting observations 74 | 75 | kkk=0; 76 | Real_local=[]; 77 | Real_localRGBD=[]; 78 | for feature_id = 1: Feature.num 79 | kkk=1; 80 | distance = norm(temp(:,feature_id) ); 81 | 82 | 83 | if localDataMatrix( feature_id , 3 )<0 || localDataMatrix( feature_id , 3 )>960 || distance >5 84 | kkk=0; 85 | end 86 | 87 | if localDataMatrix( feature_id , 4 )<0 || localDataMatrix( feature_id , 4 )>640 || distance >5 88 | kkk=0; 89 | end 90 | 91 | if temp(3, feature_id ) < 0 92 | kkk=0; 93 | end 94 | 95 | 96 | if kkk==1 97 | Real_local=[Real_local; localDataMatrix( feature_id , : ) ]; 98 | Real_localRGBD=[Real_localRGBD; localRGBD( feature_id, : ) ]; 99 | end 100 | 101 | end 102 | 103 | %%%%% Pick out unexisting observations 104 | RGBD_data=[RGBD_data; Real_localRGBD ]; 105 | 106 | Camera.Data=[Camera.Data; Real_local]; 107 | 108 | %Camera.Data.XZ=[Camera.Data.XZ Dxz']; 109 | %Camera.Data.YZ=[Camera.Data.YZ Dyz']; 110 | 111 | Camera.timehistory=[Camera.timehistory loadtime_Camera]; 112 | loadtime_Camera=loadtime_Camera+Camera.timestep; 113 | 114 | pointer=pointer+1; 115 | end 116 | 117 | Camera.NumOfFrames=pointer; 118 | 119 | 120 | end 121 | 122 | -------------------------------------------------------------------------------- /VINS_DataGenerator/A_ProduceData/Fun_initilization.m: -------------------------------------------------------------------------------- 1 | function [ IMU, Camera, Time ] = Fun_initilization 2 | 3 | 4 | IMU=struct; 5 | Camera=struct; 6 | 7 | 8 | 9 | IMU.frequency=100; %Hz 10 | IMU.timestep=0.01; 11 | IMU.timehistory=[]; 12 | %IMU.gyro.bias=[-0.000728023252950725;-0.000617546436714006;0.000302369770010283]; 13 | %IMU.gyro.bias_SigmaDynamics=0.0004; 14 | %IMU.gyro.bias_CovDynamics=IMU.gyro.bias_SigmaDynamics^2*eye(3); 15 | %IMU.gyro.bias_invCovDynamics=inv(IMU.gyro.bias_CovDynamics); 16 | gyro_noise_sigma=0;%0.0007; % rad/s 17 | IMU.gyro.noise_cov= gyro_noise_sigma^2*eye(3); 18 | IMU.gyro.noise_inf=inv(IMU.gyro.noise_cov); 19 | IMU.gyro.Data=[]; % will be 3 * NumberOfTimeSteps 20 | 21 | IMU.gyro.bias=[0;0;0]; 22 | IMU.gyro.bias_sigma= 0 ;%0.0004; 23 | 24 | 25 | IMU.acc.bias=[0;0;0]; 26 | IMU.acc.bias_sigma= 0;%0.012; 27 | 28 | 29 | acc_noise_sigma= 0;%0.019; % m/(s^2) 30 | IMU.acc.noise_cov=acc_noise_sigma^2*eye(3); 31 | IMU.acc.noise_inf=inv(IMU.acc.noise_cov); 32 | IMU.acc.Data=[]; % will be 3 * NumberOfTimeSteps 33 | 34 | Camera.frequency=2.5; % Hz 35 | Camera.timestep=1/Camera.frequency; 36 | Camera.noise_sigma=1/315; % 1 pixel noise, focal length is 315 pixel 37 | Camera.Data=[]; 38 | %Camera.Data.XZ=[]; 39 | %Camera.Data.YZ=[]; 40 | Camera.timehistory=[]; 41 | 42 | 43 | Time.start=0; 44 | Time.end=20; 45 | 46 | 47 | 48 | end 49 | 50 | -------------------------------------------------------------------------------- /VINS_DataGenerator/A_ProduceData/SimulatorVehicle.m: -------------------------------------------------------------------------------- 1 | function [ Vehicle ] = SimulatorVehicle( t ) 2 | 3 | 4 | % Vehicle.position= [ 5*cos(0.3*t); 4*sin(0.2*t); 2*sin(0.2*t+1) ]; 5 | % dp=[-1.5*sin(0.3*t); 0.8*cos(0.2*t); 0.4*cos(0.2*t+1) ]; 6 | % ddp=[-0.45*cos(0.3*t); -0.16*sin(0.2*t); -0.08*sin(0.2*t+1)]; 7 | k=2*pi/20; 8 | 9 | Vehicle.position= [ 5*cos(k*t); 4*sin(k*t); 0.4*sin(k*t) ]; 10 | dp=[-5*sin(k*t); 4*cos(k*t); 0.4*cos(k*t) ]*k; 11 | ddp=[-5*cos(k*t);-4*sin(k*t);-0.4*sin(k*t) ]*k^2; 12 | 13 | 14 | Vehicle.OneLoopTime = 2*pi/k; 15 | 16 | 17 | 18 | theta=[ 0.2*cos(t) ; 0.3*sin(t) ; k*t ];%-0.3*t ]; 19 | dtheta=[- 0.2*sin(t) ; 0.3*cos(t); k ]; 20 | 21 | R0 = [0 0 -1; -1 0 0; 0 1 0 ]; 22 | 23 | 24 | Vehicle.orientation=EularAngle( theta )* R0 ; 25 | Vehicle.lv=Vehicle.orientation'* dp; 26 | Vehicle.dp=dp; 27 | 28 | Vehicle.av=R0' *( J_rota(theta)*dtheta ); 29 | Vehicle.accLocal= Vehicle.orientation'* (ddp -[0;0; -9.8] ); 30 | 31 | 32 | 33 | 34 | 35 | end 36 | 37 | -------------------------------------------------------------------------------- /VINS_DataGenerator/Factors/Factor_IMU.m: -------------------------------------------------------------------------------- 1 | function [ Jimu_ri, Jimu_rj, ErrorVector_imu_rirj ] = Factor_IMU( xi, xj, IMU_preintegrationIJ, FrameTime ) 2 | 3 | J1=IMU_preintegrationIJ(1:3,1:3); 4 | J2=IMU_preintegrationIJ(1:3,4); 5 | J3=IMU_preintegrationIJ(1:3,5); 6 | 7 | hatXj=prediction(xi,J1,J2,J3, FrameTime); 8 | 9 | [ Error , ErrorVector_imu_rirj]=Special_minus( hatXj , fun_x2X(xj) ); 10 | 11 | % to do Jacobians 12 | [ Jreinvserse ] = Jr_inverse( ErrorVector_imu_rirj ); 13 | J1t=J1'; 14 | %K= [ J1t zeros(3,6); J1t*skew(J2) J1t zeros(3,3); J1t*skew(J3) FrameTime*J1t zeros(3,3) ]; 15 | K= [ J1t zeros(3,6); -J1t*skew(J2) J1t zeros(3,3); -J1t*skew(J3) FrameTime*J1t J1t ]; 16 | 17 | 18 | Ri=xi(1:3,1:3); 19 | vi=xi(1:3,4); 20 | pi=xi(1:3,5); 21 | 22 | Rj=xj(1:3,1:3); 23 | vj=xj(1:3,4); 24 | pj=xj(1:3,5); 25 | 26 | 27 | 28 | T= [ Ri' zeros(3,6); -skew(vi)*Ri eye(3) zeros(3,3); -Ri'*skew(pi) zeros(3,3) Ri' ]; 29 | Tj=[ Rj' zeros(3,6); -skew(vj)*Rj eye(3) zeros(3,3); -Rj'*skew(pj) zeros(3,3) Rj' ]; 30 | adjj=ad_G_inverse(Error); 31 | 32 | Jimu_ri=Jreinvserse*K*T; 33 | Jimu_rj=-Jreinvserse*adjj*Tj; 34 | end 35 | 36 | function hatXj=prediction(xi,J1,J2,J3, FrameTime) 37 | g=[0 ; 0; -9.8]; 38 | Ri=xi(1:3,1:3); 39 | hatRj=Ri*J1; 40 | 41 | vi_G=Ri*xi(1:3,4); 42 | hatvj_G=vi_G+Ri*J2+FrameTime*g; 43 | 44 | pi=xi(1:3,5); 45 | hatpj=pi+FrameTime^2/2*g+FrameTime*vi_G+Ri*J3; 46 | 47 | hatXj=[ hatRj hatvj_G hatpj ]; 48 | end 49 | 50 | function [Error,error]=Special_minus( X1 , X2 ) 51 | 52 | R2=X2(1:3,1:3); 53 | R1=X1(1:3,1:3); 54 | 55 | theta1=so3_log( R2'*R1 ); 56 | 57 | dv_G=R2'*( X1(1:3,4 )-X2(1:3,4 ) ); 58 | dp=R2'*( X1(1:3,5 )-X2(1:3,5 ) ); 59 | 60 | theta2=jacor_inverse(-theta1)*dv_G; 61 | theta3=jacor_inverse(-theta1)*dp; 62 | 63 | error=[theta1; theta2;theta3 ]; 64 | 65 | Error= [R2'*R1 dv_G dp ]; 66 | 67 | end -------------------------------------------------------------------------------- /VINS_DataGenerator/Factors/Factor_Prior.m: -------------------------------------------------------------------------------- 1 | function [ Jprior_r0,ErrorVector_prior ] = Factor_Prior( x0, Priorx0 ) 2 | 3 | %addpath('../MathOperation/'); 4 | 5 | R0=x0(1:3,1:3); 6 | p0=x0(1:3,5); 7 | 8 | R_prior=Priorx0(1:3,1:3); 9 | p_prior=Priorx0(1:3,5); 10 | 11 | 12 | dR=R0*R_prior'; 13 | dP=p0-R0*R_prior'*p_prior; 14 | 15 | theta1=so3_log( dR); 16 | theta2= jacor_inverse( -theta1 )*(dP); 17 | ErrorVector_prior=[theta1;theta2]; 18 | 19 | 20 | %adg_inverse=[ dR' zeros(3,3) ; -dR'*skew(dP) dR' ]; 21 | 22 | 23 | Jprior_r0= [ dR' zeros(3,6); -dR'*skew(dP) zeros(3,3) dR' ] ; 24 | 25 | 26 | 27 | 28 | 29 | end 30 | 31 | -------------------------------------------------------------------------------- /VINS_DataGenerator/Factors/Factor_Vision.m: -------------------------------------------------------------------------------- 1 | function [ Jvision_ri, Jvision_fk, ErrorVector_vision_ri_fk ] = Factor_Vision( xi, fk, Vision_ik ) 2 | 3 | R=xi(1:3,1:3); 4 | p=xi(1:3,5); 5 | 6 | hL =R'*(fk-p); 7 | hL1=hL(1); 8 | hL2=hL(3); 9 | hL3=hL(3); 10 | ErrorVector_vision_ri_fk=[ hL(1)/hL(3)-Vision_ik(1); hL(2)/hL(3)-Vision_ik(2) ]; 11 | 12 | H= 1/(hL3^2) * [ hL3 0 -hL1; 0 hL3 -hL2 ]; 13 | 14 | hi= [R'*skew(fk) zeros(3,3) -R']; 15 | Jvision_ri=H*hi; 16 | 17 | hk= R'; 18 | Jvision_fk= H*hk; 19 | 20 | 21 | end 22 | 23 | -------------------------------------------------------------------------------- /VINS_DataGenerator/Fun_DoPreintegration.m: -------------------------------------------------------------------------------- 1 | function [ Measurement_IMU, IMUbiasWalk_inf] = Fun_DoPreintegration( LinearizedPoint, IMU, Camera ) 2 | 3 | 4 | 5 | 6 | Measurement_IMU=cell( 1, Camera.NumOfFrames-1); 7 | 8 | for i=0:Camera.NumOfFrames-2 9 | 10 | accbias_es=LinearizedPoint.robot( 3*i+1 : 3*(i+1), 7 ); 11 | gyrobias_es=LinearizedPoint.robot( 3*i+1 : 3*(i+1), 8 ); 12 | 13 | 14 | [ IMU_DataIJ ] = Fun_IMUdataIJ( i,i+1, Camera.timestep, IMU ); 15 | 16 | IMUsettings=struct; 17 | IMUsettings.gyro_noise_cov= IMU.gyro.noise_cov; 18 | IMUsettings.acc_noise_cov=IMU.acc.noise_cov; 19 | IMUsettings.accbias_es=accbias_es; 20 | IMUsettings.gyrobias_es=gyrobias_es; 21 | IMUsettings.gyro.bias_cov= ( eye(3)* IMU.gyro.bias_sigma^2 ); 22 | IMUsettings.acc.bias_cov= ( eye(3)* IMU.acc.bias_sigma^2 ); 23 | 24 | % accbias_es =zeros(3,1); 25 | % gyrobias_es = zeros(3,1); 26 | 27 | [ value, inf ] = Fun_PreIntegration_bias( IMU_DataIJ, IMUsettings, accbias_es, gyrobias_es ); 28 | 29 | IMUbiasWalk_inf= (1/Camera.timestep)*blkdiag( eye(3)*(1/(IMU.acc.bias_sigma^2 )) , eye(3)*(1/(IMU.gyro.bias_sigma^2) ) ) ; 30 | 31 | 32 | value.dt = Camera.timestep; 33 | value.bw=gyrobias_es; 34 | value.ba=accbias_es; 35 | 36 | Measurement_IMU{i+1}.value=value; 37 | Measurement_IMU{i+1}.inf=inf; 38 | Measurement_IMU{i+1}.fromPose=['pose' num2str(i)]; 39 | Measurement_IMU{i+1}.toPose=['pose' num2str(i+1)]; 40 | end 41 | 42 | 43 | 44 | 45 | end 46 | 47 | -------------------------------------------------------------------------------- /VINS_DataGenerator/Fun_InitialEstimate.m: -------------------------------------------------------------------------------- 1 | function [ InitialEstimate ] = Fun_InitialEstimate( RealVehicle, Settings ) 2 | 3 | NumOfFrames=Settings.NumOfFrames; 4 | 5 | InitialEstimate=[]; 6 | BS=Settings.BS; 7 | 8 | for i=0:1:NumOfFrames-1 9 | 10 | orientation= RealVehicle.orientation( 1:3, i*3*BS+1 : i*3*BS+3 ); 11 | velocity_L=RealVehicle.localvelocity( :, i*BS+1 ); 12 | position=RealVehicle.position( :, i*BS+1 ); 13 | 14 | accbias=RealVehicle.acc_bias_history(1:3, i*BS+1 ); 15 | gyrobias=RealVehicle.gyro_bias_history(1:3, i*BS+1 ); 16 | 17 | index=[i ]; 18 | Index=repmat(index, 3, 1 ); 19 | 20 | tempA= [orientation position velocity_L Index accbias gyrobias]; 21 | 22 | InitialEstimate= [InitialEstimate; tempA ]; 23 | end 24 | 25 | 26 | 27 | 28 | end 29 | 30 | -------------------------------------------------------------------------------- /VINS_DataGenerator/Fun_Optimization_GN.m: -------------------------------------------------------------------------------- 1 | function [ newLinearizedPoint ] = Fun_Optimization_GN( Feature, IMUfactorMeasurement, Camera, Inf_Matrix, LinearizedPoint ) 2 | 3 | addpath('Factors/'); 4 | addpath('MathOperation/'); 5 | 6 | 7 | 8 | 9 | num.robotstate=Camera.NumOfFrames; 10 | num.feature=Feature.num; 11 | num.observation=size(Camera.Data,1); 12 | 13 | 14 | [ SparseJacobian_index_row, SparseJacobian_index_col, SparseJacobian_values ]= CreatSparseJacobian_rowcol_index( Camera,Feature ); 15 | FullErrorVector = ones( 6 + 9*(num.robotstate-1)+2*num.observation,1 ); 16 | 17 | 18 | 19 | 20 | iteration_max=10; 21 | for iteration=1:iteration_max 22 | 23 | 24 | [ SparseJacobian_values, FullErrorVector ] = Fun_GetJacobianError( LinearizedPoint, IMUfactorMeasurement, Camera, SparseJacobian_values , FullErrorVector, Feature ); 25 | SparseJacobian_matrix=sparse( SparseJacobian_index_row, SparseJacobian_index_col, SparseJacobian_values ); 26 | 27 | A=SparseJacobian_matrix'*Inf_Matrix*SparseJacobian_matrix; 28 | 29 | dx=-A\(SparseJacobian_matrix'*Inf_Matrix*FullErrorVector); 30 | 31 | 32 | LinearizedPoint=Update_fullstate(LinearizedPoint, dx, num.robotstate, num.feature ); 33 | 34 | 35 | 36 | FullErrorVector'*Inf_Matrix*FullErrorVector 37 | 38 | end 39 | 40 | 41 | newLinearizedPoint=LinearizedPoint; 42 | 43 | 44 | 45 | end 46 | 47 | -------------------------------------------------------------------------------- /VINS_DataGenerator/Fun_ProduceData.m: -------------------------------------------------------------------------------- 1 | function [ Settings, Camera, RealVehicle, Feature, IMU, RGBD_data] = Fun_ProduceData 2 | 3 | addpath('MathOperation/'); 4 | addpath('A_ProduceData/'); 5 | 6 | 7 | [IMU, Camera, Time]=Fun_initilization; 8 | [ Feature ] = Fun_generateFeatures; 9 | [ IMU,Camera,RealVehicle, RGBD_data ] = Fun_getData( Time,IMU,Camera,Feature ); 10 | 11 | 12 | 13 | 14 | FrameTime=Camera.timestep; 15 | NumOfFrames=Camera.NumOfFrames; 16 | 17 | IndexFeatures=unique(Camera.Data(:,2)); 18 | NumOfFeatures=size(IndexFeatures,1); 19 | 20 | Settings.FrameTime=FrameTime; 21 | Settings.NumOfFrames=NumOfFrames; 22 | Settings.NumOfFeatures=NumOfFeatures; 23 | Settings.BS=floor(Camera.timestep/IMU.timestep); 24 | 25 | 26 | end 27 | 28 | -------------------------------------------------------------------------------- /VINS_DataGenerator/Integration.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS_DataGenerator/Integration.zip -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/EularAngle.m: -------------------------------------------------------------------------------- 1 | function y = EularAngle( X ) 2 | 3 | a=X(1); %x 4 | b=X(2); %y 5 | c=X(3); %z 6 | 7 | y=[cos(c)*cos(b) -sin(c)*cos(a)+cos(c)*sin(b)*sin(a) sin(c)*sin(a)+cos(c)*cos(a)*sin(b);... 8 | sin(c)*cos(b) cos(c)*cos(a)+sin(a)*sin(b)*sin(c) -cos(c)*sin(a)+sin(b)*sin(c)*cos(a); ... 9 | -sin(b) cos(b)*sin(a) cos(b)*cos(a)]; 10 | 11 | 12 | 13 | 14 | end 15 | 16 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/J_rota.m: -------------------------------------------------------------------------------- 1 | function y=J_rota(X) 2 | a=X(1); 3 | b=X(2); 4 | y=[1 0 -sin(b);... 5 | 0 cos(a) cos(b)*sin(a);... 6 | 0 -sin(a) cos(b)*cos(a) 7 | ]; -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/Jr.m: -------------------------------------------------------------------------------- 1 | function [ Jre ] = Jr( e ) 2 | 3 | e1=e(1:3); 4 | e2=e(4:6); 5 | e3=e(7:9); 6 | 7 | jr1= jaco_r( e1 ); 8 | kr2= Kr(e1,e2); 9 | kr3= Kr(e1,e3); 10 | 11 | Jre= [jr1 zeros(3,6) ;... 12 | kr2 jr1 zeros(3,3);... 13 | kr3 zeros(3,3) jr1 ]; 14 | 15 | end 16 | 17 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/Jr_inverse.m: -------------------------------------------------------------------------------- 1 | function [ Jreinvserse ] = Jr_inverse( e ) 2 | 3 | e1=e(1:3); 4 | e2=e(4:6); 5 | e3=e(7:9); 6 | 7 | j1=jacor_inverse(e1); 8 | Qr2=Kr( e1, e2 ); 9 | Qr3=Kr( e1, e3 ); 10 | 11 | Jreinvserse=[ j1 zeros(3,6) ;... 12 | -j1*Qr2*j1 j1 zeros(3,3);... 13 | -j1*Qr3*j1 zeros(3,3) j1 ]; 14 | 15 | 16 | 17 | 18 | 19 | 20 | end 21 | 22 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/Kr.m: -------------------------------------------------------------------------------- 1 | function [ Kr_value ] = Kr( theta1, theta2 ) 2 | 3 | 4 | Kr_value=Kl(-theta1,-theta2); 5 | 6 | 7 | 8 | 9 | end 10 | 11 | 12 | function Kl_value=Kl(x1,x2) 13 | 14 | 15 | theta=norm(x1); 16 | 17 | 18 | Kl_value=1/2*skew(x2)+(theta-sin(theta))/theta^3*(skew(x1)*skew(x2)+skew(x2)*skew(x1)+skew(x1)*skew(x2)*skew(x1))... 19 | -(1-theta^2/2-cos(theta))/theta^4*(skew(x1)*skew(x1)*skew(x2)+skew(x2)*skew(x1)*skew(x1)-3*skew(x1)*skew(x2)*skew(x1) )... 20 | -1/2*( (1-theta^2/2-cos(theta))/theta^4-3*(theta-sin(theta)-theta^3/6)/theta^5)*(skew(x1)*skew(x2)*skew(x1)*skew(x1)+skew(x1)*skew(x1)*skew(x2)*skew(x1)); 21 | 22 | 23 | 24 | 25 | 26 | end -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/Update_fullstate.m: -------------------------------------------------------------------------------- 1 | function [ newLinearizedPoint ]=Update_fullstate(LinearizedPoint, dx , num_robot,num_feature ) 2 | newLinearizedPoint=LinearizedPoint; 3 | 4 | for ii=1:num_robot 5 | index_ii= 1 + 3*( ii-1 ) : 3+3*( ii-1 ); 6 | index_robot= 1+9*(ii-1) : 9 *(ii-1)+9; 7 | newLinearizedPoint.robot( index_ii , 1:5 )= update_robotstate(LinearizedPoint.robot( index_ii , 1:5 ), dx( index_robot ) ); 8 | end 9 | 10 | feature_matrix=LinearizedPoint.feature(1:3,:); 11 | feature_vector=feature_matrix(:); 12 | feature_vector=feature_vector+dx( 9*num_robot+1:end); 13 | 14 | feature_matrix=reshape(feature_vector, 3, num_feature ); 15 | 16 | 17 | newLinearizedPoint.feature( 1:3, : )=feature_matrix; 18 | 19 | 20 | end 21 | 22 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/ad_G.m: -------------------------------------------------------------------------------- 1 | function [ adX ] = ad_G( X ) 2 | 3 | 4 | R=X(1:3,1:3); 5 | v_G=X(1:3,4); 6 | p=X(1:3,5); 7 | 8 | 9 | adX=[ R zeros(3,6); ... 10 | skew(v_G)*R R zeros(3,3);... 11 | skew(p)*R zeros(3,3) R ]; 12 | 13 | 14 | end 15 | 16 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/ad_G_inverse.m: -------------------------------------------------------------------------------- 1 | function [ adX_inverse ] = ad_G_inverse( X ) 2 | 3 | 4 | R=X(1:3,1:3); 5 | v_G=X(1:3,4); 6 | p=X(1:3,5); 7 | 8 | 9 | adX_inverse=[ R' zeros(3,6); ... 10 | -R'*skew(v_G) R' zeros(3,3);... 11 | -R'*skew(p) zeros(3,3) R' ]; 12 | 13 | 14 | end 15 | 16 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/fun_x2X.m: -------------------------------------------------------------------------------- 1 | function [ X ] = fun_x2X( x ) 2 | 3 | 4 | X=x; 5 | 6 | X(1:3,4)=x(1:3,1:3)*x(1:3,4); 7 | 8 | end 9 | 10 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/jaco_r.m: -------------------------------------------------------------------------------- 1 | function result = jaco_r( delta ) 2 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3 | % Function: Overloading function. 4 | % Method1: I(3x3) - (1-cos(theta))*skew(delta)/norm(delta)^2 + ... 5 | % (norm(delta)-sin(norm(delta)))*skew(delta)^2/norm(delta)^3 6 | % Input1: delta: 3x1 vector 7 | % Returns1: result: See the document 8 | 9 | % Method2: See the document 10 | % Input2: delta: 6+3N vector 11 | % Returns2: result: See the document 12 | 13 | % Author: Jingwei Song. 04/06/2016 14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 15 | N= (size(delta,1)-3)/3; 16 | 17 | if(size(delta,1) == 3) 18 | function_mode = 1; 19 | else 20 | function_mode = 2; 21 | end 22 | 23 | if(function_mode == 1) 24 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 25 | % Function1 26 | theta = norm(delta); 27 | if(theta < 0.00000001) 28 | result = eye(3,3); 29 | else 30 | result = eye(3,3) - (1-cos(theta))*skew(delta)/theta^2 + (theta-sin(theta))*skew(delta)^2/norm(delta)^3; 31 | end 32 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | else 34 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 35 | % Function2 36 | result = zeros(3+3*N,3+3*N); 37 | s_theta = delta(1:3,1); 38 | for i = 1 : N+1 % Set diagonal to J_r(s_theta) 39 | result(3*i-2:3*i,3*i-2:3*i) = jaco_r(s_theta); 40 | end 41 | for i = 1 : N 42 | s_temp = delta(3*i+1:3*i+3,1); 43 | result(3*i+1:3*i+3,1:3) = K_r_x1x2(s_theta, s_temp); 44 | end 45 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 46 | end 47 | 48 | 49 | end 50 | 51 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/jacor_inverse.m: -------------------------------------------------------------------------------- 1 | function [ jrx_inverse ] = jacor_inverse( dx ) 2 | phi=sqrt(dx'*dx); 3 | 4 | if phi<0.00001 5 | jrx_inverse=eye(3); 6 | else 7 | sp=skew(dx); 8 | jrx_inverse=eye(3)+0.5*sp+(1/phi^2 - (1+cos(phi))/(2*phi*sin(phi)))*sp*sp; 9 | end 10 | 11 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/skew.m: -------------------------------------------------------------------------------- 1 | function y = skew( x ) 2 | % compute the skew symetric matrix 3 | % given input 3x1 vector x 4 | y=[0 -x(3) x(2);... 5 | x(3) 0 -x(1);... 6 | -x(2) x(1) 0]; 7 | end 8 | 9 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/so3_exp.m: -------------------------------------------------------------------------------- 1 | function [ result ] = so3_exp( x ) 2 | % compute the exponential mapping of R^3 to SO(3) 3 | % use sparse matrix 4 | N = size(x,1); 5 | %if N == 3 6 | theta=norm(x); 7 | if theta==0 8 | result=eye(3); 9 | else 10 | omega =x/theta; 11 | result=eye(3,3) + sin(theta) * skew(omega) + (1 - cos(theta))*skew(omega)^2; 12 | end 13 | 14 | %else 15 | % todo, compute exp() of R^6 to SE(3)? 16 | % m = (N-3)/3; 17 | % result=speye(3+m); 18 | % result(1:3,1:3)=Exp( x(1:3)); 19 | %end 20 | end 21 | 22 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/so3_hatinv.m: -------------------------------------------------------------------------------- 1 | function v = so3_hatinv(xi) 2 | v = [xi(3,2); xi(1,3); xi(2,1)]; -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/so3_log.m: -------------------------------------------------------------------------------- 1 | function f = so3_log(R, varargin) 2 | 3 | if (nargin>1) 4 | if (norm(R-eye(3),'fro') < 2*eps) 5 | f = zeros(3,1); 6 | return 7 | end 8 | end 9 | 10 | phi = acos(1/2*(trace(R)-1)); 11 | 12 | if (nargin>1) 13 | if (abs(phi) < 1e-10) 14 | f = zeros(3,1); 15 | return 16 | end 17 | end 18 | 19 | if norm(R-eye(3))>0.0000000001 20 | f = so3_hatinv(phi/(2*sin(phi))*(R-R')); 21 | else 22 | f=[0;0;0]; 23 | end -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/test.m: -------------------------------------------------------------------------------- 1 | 2 | clc 3 | clear 4 | 5 | dx=randn(3,1); 6 | 7 | jaco_r(dx)*jacor_inverse(dx) 8 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/update_Features.m: -------------------------------------------------------------------------------- 1 | function [ f ] = update_Features( f,df ) 2 | 3 | 4 | f=f+df; 5 | 6 | end 7 | 8 | -------------------------------------------------------------------------------- /VINS_DataGenerator/MathOperation/update_robotstate.m: -------------------------------------------------------------------------------- 1 | function [ x ] = update_robotstate( x, dx ) 2 | 3 | orientation=x(1:3,1:3); 4 | velocity_l=x(1:3,4); 5 | position=x(1:3,5); 6 | 7 | 8 | orientation=so3_exp(dx(1:3))*orientation; 9 | velocity_l=velocity_l+dx(4:6); 10 | position=so3_exp(dx(1:3))*position+jaco_r(-dx(1:3))*dx(7:9); 11 | 12 | 13 | x(1:3,1:3)=orientation; 14 | x(1:3,4)=velocity_l; 15 | x(1:3,5)=position; 16 | end 17 | 18 | 19 | -------------------------------------------------------------------------------- /VINS_DataGenerator/OptimizationProcess/CreatSparseJacobian_rowcol_index.m: -------------------------------------------------------------------------------- 1 | function [ SparseJacobian_index_row, SparseJacobian_index_col, SparseJacobian_values ]= CreatSparseJacobian_rowcol_index( Camera,Feature ) 2 | 3 | num.robotstate=Camera.NumOfFrames; 4 | num.feature=Feature.num; 5 | num.observation=size(Camera.Data,1); 6 | 7 | SparseJacobian_index_row=ones( 6*9+9^2*2*(num.robotstate-1)+24*num.observation,1); 8 | SparseJacobian_index_col=ones( 6*9+9^2*2*(num.robotstate-1)+24*num.observation,1); 9 | SparseJacobian_values=ones( 6*9+9^2*2*(num.robotstate-1)+24*num.observation,1); 10 | 11 | % Prior Factor 12 | s=[1:6]'; 13 | ss=[1:9]; 14 | Mpr_r= repmat(s,1,9); 15 | Mpr_c= repmat(ss,6, 1 ); 16 | SparseJacobian_index_row(1:54)=Mpr_r(:); 17 | SparseJacobian_index_col(1:54)=Mpr_c(:); 18 | 19 | % IMU Factor 20 | IndexMatrix1=[1:9]'; 21 | IndexMatrix1=repmat(IndexMatrix1,1,18 ); 22 | IndexMatrix2=[1:18]; 23 | IndexMatrix2=repmat(IndexMatrix2,9,1); 24 | for i=0:num.robotstate-2 25 | SparseJacobian_index_row(54+ 2*i*9^2+1 : 54+ 2*i*9^2+81*2)= [IndexMatrix1(:)+6+9*i]; 26 | SparseJacobian_index_col(54+ 2*i*9^2+1 : 54+ 2*i*9^2+81*2)= [IndexMatrix2(:)+9*i]; 27 | end 28 | 29 | A1= [ones(1,9); 2*ones(1,9)]; B1=[1 1 1; 2 2 2]; 30 | C1=[1:9;1:9]; D1=[1 2 3; 1 2 3]; 31 | 32 | %Vision Factor 33 | for index_ob=1:num.observation 34 | 35 | Ri_index=Camera.Data(index_ob,1); 36 | fk_index=Camera.Data(index_ob,2); 37 | 38 | row_r= 6+9*(num.robotstate-1)+A1+ 2*(index_ob-1) ; 39 | row_f= 6+9*(num.robotstate-1)+B1+ 2*(index_ob-1) ; 40 | 41 | col_r=9*(Ri_index )+C1; 42 | col_f=9*(num.robotstate)+3*(fk_index-1)+D1; 43 | 44 | SparseJacobian_index_row(54+ 2*( num.robotstate-1 )*9^2+24*(index_ob-1)+1 : 54+ 2*(num.robotstate-1)*9^2+24*(index_ob-1)+24)=[row_r(:); row_f(:)]; 45 | SparseJacobian_index_col(54+ 2*( num.robotstate-1 )*9^2+24*(index_ob-1)+1 : 54+ 2*(num.robotstate-1)*9^2+24*(index_ob-1)+24)=[ col_r(:); col_f(:) ]; 46 | 47 | end 48 | 49 | 50 | end 51 | 52 | -------------------------------------------------------------------------------- /VINS_DataGenerator/OptimizationProcess/Fun_GetJacobianError.m: -------------------------------------------------------------------------------- 1 | function [ SparseJacobian_values, FullErrorVector ] = Fun_GetJacobianError( LinearizedPoint, IMUfactorMeasurement, Camera, SparseJacobian_values , FullErrorVector, Feature ) 2 | 3 | num.robotstate=Camera.NumOfFrames; 4 | num.feature=Feature.num; 5 | num.observation=size(Camera.Data,1); 6 | FrameTime=Camera.timestep; 7 | 8 | 9 | % Frior Factor 10 | Prior=LinearizedPoint.robot(1:3,1:5); 11 | x0=LinearizedPoint.robot(1:3, 1:5); 12 | [ Jprior_r0,ErrorVector_prior ] = Factor_Prior( x0, Prior ); 13 | FullErrorVector(1:6,1)=ErrorVector_prior; 14 | SparseJacobian_values(1:54) = Jprior_r0(:); 15 | 16 | % IMU Factor 17 | for imu_index=0:num.robotstate-2 18 | xi=LinearizedPoint.robot( 1+3*imu_index :3+ 3*imu_index ,1:5); 19 | xj=LinearizedPoint.robot( 1+3*imu_index+3 :3+ 3*imu_index+3 ,1:5); 20 | IMU_preintegrationIJ=IMUfactorMeasurement( 1+3*imu_index :3+ 3*imu_index,1:5); 21 | 22 | [ Jimu_ri, Jimu_rj, ErrorVector_imu_rirj ] = Factor_IMU( xi, xj, IMU_preintegrationIJ, FrameTime ); 23 | FullErrorVector( 6 + 9*imu_index+ 1: 6 +9*imu_index+ 9 )=ErrorVector_imu_rirj; 24 | J_imu=[ Jimu_ri, Jimu_rj ]; 25 | SparseJacobian_values( 54+ 2*imu_index*9^2+1 : 54+ 2*imu_index*9^2+81*2 ) =J_imu(:); 26 | 27 | end 28 | 29 | % Vision Factor 30 | for vision_index=1:num.observation 31 | Ri_index=Camera.Data(vision_index,1); 32 | fk_index=Camera.Data(vision_index,2); 33 | 34 | xi=LinearizedPoint.robot( 1+3*Ri_index :3+ 3*Ri_index ,1:5); 35 | fk_index_row= find( LinearizedPoint.feature( 4,: )== fk_index ) ; 36 | fk=LinearizedPoint.feature( 1:3, fk_index_row ); 37 | Vision_ik=Camera.Data(vision_index, 3:4); 38 | 39 | [ Jvision_ri, Jvision_fk, ErrorVector_vision_ri_fk ] = Factor_Vision( xi, fk, Vision_ik ); 40 | % ErrorVector_vision_ri_fk 41 | J_sub=[Jvision_ri, Jvision_fk]; 42 | FullErrorVector( 6+9*(num.robotstate-1)+ 2*(vision_index-1)+1 : 6+9*(num.robotstate-1)+ 2*(vision_index-1)+2 ) =ErrorVector_vision_ri_fk; 43 | 44 | index=54+ 2*( num.robotstate-1 )*9^2+24*(vision_index-1)+1 : 54+ 2*(num.robotstate-1)*9^2+24*(vision_index-1)+24; 45 | SparseJacobian_values( index )=J_sub(:); 46 | end 47 | 48 | 49 | 50 | end 51 | 52 | -------------------------------------------------------------------------------- /VINS_DataGenerator/OptimizationProcess/Untitled.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | num.robotstate=Camera.NumOfFrames; 4 | num.feature=Feature.num; 5 | num.observation=size(Camera.Data,1); 6 | FrameTime=Camera.timestep; 7 | 8 | [ SparseJacobian_index_row, SparseJacobian_index_col, SparseJacobian_values ]= CreatSparseJacobian_rowcol_index( Camera,Feature ); 9 | FullErrorVector = ones( 6 + 9*(num.robotstate-1)+2*num.observation,1 ); 10 | 11 | 12 | % Prior Factor 13 | 14 | 15 | iteration_max=10; 16 | for iteration=1:iteration_max 17 | 18 | 19 | [ SparseJacobian_values, FullErrorVector ] = Fun_GetJacobianError( LinearizedPoint, IMUfactorMeasurement, Camera, SparseJacobian_values , FullErrorVector, Feature ); 20 | SparseJacobian_matrix=sparse( SparseJacobian_index_row, SparseJacobian_index_col, SparseJacobian_values ); 21 | 22 | A=SparseJacobian_matrix'*Inf_Matrix*SparseJacobian_matrix; 23 | 24 | dx=A\FullErrorVector; 25 | 26 | LinearizedPoint=Update_fullstate(LinearizedPoint, dx, num.robotstate, num.feature ); 27 | 28 | 29 | end 30 | -------------------------------------------------------------------------------- /VINS_DataGenerator/Untitled.m: -------------------------------------------------------------------------------- 1 | t=0:0.01:1; 2 | x1=sin(t);x2=sin(t*1.1);x3=cos(t); 3 | 4 | X=[x1;x2;x3]; -------------------------------------------------------------------------------- /VINS_DataGenerator/main.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | close all 4 | 5 | addpath('MathOperation/'); 6 | addpath('A_ProduceData/'); 7 | addpath('Factors/'); 8 | addpath('OptimizationProcess/'); 9 | 10 | % Produce necessary settings&measurements for optimization, and ground truth for comparison 11 | tic 12 | [ Settings, Camera, RealVehicle, Feature, IMU, RGBD_data] = Fun_ProduceData; 13 | %%% Get Initial Estimate 14 | LinearizedPoint=struct; 15 | [ LinearizedPoint.robot ] = Fun_InitialEstimate( RealVehicle, Settings ); 16 | LinearizedPoint.feature= [Feature.position; 1:Feature.num]; 17 | 18 | %%%%%%%%%%%%%%%%%%%%%%%% Preintegration based on the current bias estimate 19 | [ Measurement_IMU, IMUbiasWalk_inf ] =Fun_DoPreintegration( LinearizedPoint, IMU, Camera ); 20 | Measurement_Vision = Camera.Data; 21 | toc 22 | 23 | 24 | num_robot = size(LinearizedPoint.robot, 1)/3; 25 | RobotState = cell(1 ,num_robot ); 26 | 27 | for i=1:num_robot 28 | RobotState{i}.id = ['pose' num2str(i-1)]; 29 | RobotState{i}.pose = LinearizedPoint.robot(3*i-2:3*i,1:4 ); 30 | RobotState{i}.velocity = LinearizedPoint.robot(3*i-2:3*i,5 ); 31 | RobotState{i}.ba= LinearizedPoint.robot(3*i-2:3*i,7 ); 32 | RobotState{i}.bw= LinearizedPoint.robot(3*i-2:3*i,8 ); 33 | end 34 | 35 | for i=1: Feature.num 36 | landmark_id = ['landmark' num2str(i)]; 37 | Landmarks.(landmark_id) = Feature.position( 1:3, i); 38 | 39 | end 40 | 41 | clear Camera Feature GroundTruth i IMU landmark_id LinearizedPoint num_robot RealVehicle Settings -------------------------------------------------------------------------------- /VINS_DataGenerator/matlab.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS_DataGenerator/matlab.mat -------------------------------------------------------------------------------- /VINS_DataGenerator/matlab2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS_DataGenerator/matlab2.mat -------------------------------------------------------------------------------- /VINS_DataGenerator/rk4_manifold_mod.m: -------------------------------------------------------------------------------- 1 | function [ value, inf ] = rk4_manifold_mod( M, N , accbias_es, gyrobias_es) 2 | 3 | % this function is used for preintegration via RK4 method in Lie group SO(3) 4 | 5 | [~,m]=size(M.gyro); 6 | h = M.timestep; 7 | 8 | Y = cell(1,m+1); 9 | 10 | PP = cell(1,m+1); 11 | 12 | Y{1}.J1=eye(3); Y{1}.j1_w=zeros(3,3); 13 | Y{1}.J2=zeros(3,1); Y{1}.j2_w=zeros(3,3); Y{1}.j2_a=zeros(3,3); 14 | Y{1}.J3=zeros(3,1); Y{1}.j3_w=zeros(3,3); Y{1}.j3_a=zeros(3,3); 15 | 16 | PP{1}=zeros(9,9); 17 | 18 | A = cell(1,m+1); 19 | A{1} = eye(9); 20 | 21 | for j=1:m 22 | yj = Y{j}; 23 | Pj = PP{j}; 24 | if j2 22 | k1 = h*derivative_sub(j,M,yj, accbias_es, gyrobias_es); 23 | k2 = h*derivative_sub(j+0.5,M, specialadd( yj , k1/2 ), accbias_es, gyrobias_es ); 24 | k3 = h*derivative_sub(j+0.5,M, specialadd( yj , k2/2 ), accbias_es, gyrobias_es ); 25 | k4 = h*derivative_sub(j+0.9,M, specialadd( yj , k3 ), accbias_es, gyrobias_es ); 26 | Y{j+1}=specialadd( yj, (k1 + 2*k2 + 2*k3 + k4)/6 ); 27 | % Y{j+1}=specialadd( yj, k1 ); 28 | 29 | K1 = h*derivative_sub2(j,M,N,Pj, accbias_es, gyrobias_es); 30 | K2 = h*derivative_sub2(j,M,N, Pj+ K1/2 , accbias_es, gyrobias_es ); 31 | K3 = h*derivative_sub2(j,M,N, Pj + K2/2 , accbias_es, gyrobias_es); 32 | K4 = h*derivative_sub2(j,M,N, Pj + K3 ,accbias_es, gyrobias_es ); 33 | PP{j+1}= Pj + (K1 + 2*K2 + 2*K3 + K4)/6 ; 34 | 35 | else 36 | k=h*derivative_sub(j,M,yj,accbias_es, gyrobias_es); 37 | Y{j+1}=specialadd( yj, k ); 38 | 39 | K=h*derivative_sub2(j,M,N, Pj,accbias_es, gyrobias_es); 40 | PP{j+1}=PP{j}+K; 41 | end 42 | 43 | end 44 | 45 | P= PP{m+1}; 46 | 47 | value=struct; 48 | value.J1=Y{m+1}.J1; value.j1_w=Y{m+1}.j1_w; 49 | 50 | value.J2=Y{m+1}.J2; value.j2_w=Y{m+1}.j2_w; value.j2_a=Y{m+1}.j2_a; 51 | value.J3=Y{m+1}.J3; value.j3_w=Y{m+1}.j3_w; value.j3_a=Y{m+1}.j3_a; 52 | 53 | inf=inv(P); 54 | 55 | 56 | end 57 | 58 | function [kj]=derivative_sub(j, M, yj , accbias_es, gyrobias_es ) 59 | J1=yj.J1; 60 | J2=yj.J2; 61 | 62 | if norm(j -floor(j))< 0.0001 63 | wt=M.gyro(1:3,j); 64 | at=M.acc(1:3,j); 65 | elseif norm(j -floor(j))<= 0.6 66 | j = floor(j); 67 | gLL= M.gyro(1:3,j-1); 68 | gL = M.gyro(1:3,j); 69 | accLL= M.acc(1:3,j-1); 70 | accL = M.acc(1:3,j); 71 | 72 | wt = 0.5*(gL-gLL)+ gL; %0.5*M.gyro(1:3,j)+ 0.5*M.gyro(1:3,j+1); 73 | at = 0.5*(accL-accLL)+accL; %0.5*M.acc(1:3,j)+ 0.5*M.acc(1:3,j+1); 74 | else 75 | j = floor(j); 76 | gLL= M.gyro(1:3,j-1); 77 | gL = M.gyro(1:3,j); 78 | accLL= M.acc(1:3,j-1); 79 | accL = M.acc(1:3,j); 80 | wt = 1*(gL-gLL)+ gL; %0.5*M.gyro(1:3,j)+ 0.5*M.gyro(1:3,j+1); 81 | at = 1*(accL-accLL)+accL; %0.5*M.acc(1:3,j)+ 0.5*M.acc(1:3,j+1); 82 | end 83 | 84 | j1_w=yj.j1_w; 85 | j2_w=yj.j2_w; 86 | j2_a=yj.j2_a; 87 | j3_w=yj.j3_w; 88 | j3_a=yj.j3_a; 89 | 90 | 91 | 92 | kj_dJ1= wt-gyrobias_es; 93 | kj_dJ2= J1*(at-accbias_es); 94 | kj_dJ3= J2; 95 | kj_dJ1_w=-skew( wt- gyrobias_es )*j1_w-eye(3); 96 | kj_dJ2_w= -J1*skew(at-accbias_es )*j1_w; 97 | kj_dJ2_a= -J1; 98 | kj_dJ3_w= j2_w; 99 | kj_dJ3_a= j2_a; 100 | 101 | kj=[ kj_dJ1 kj_dJ2 kj_dJ3 kj_dJ1_w kj_dJ2_a kj_dJ2_w kj_dJ3_a kj_dJ3_w ]; 102 | 103 | end 104 | 105 | 106 | function [Kj]=derivative_sub2(j, M, N, Pj, accbias_es, gyrobias_es ) 107 | wt=M.gyro(1:3,j); 108 | at=M.acc(1:3,j); 109 | 110 | wt=wt-gyrobias_es; 111 | at=at-accbias_es; 112 | 113 | A=[ -skew(wt) zeros(3,3) zeros(3,3);... 114 | -skew(at) -skew(wt) zeros(3,3);... 115 | zeros(3,3) eye(3) -skew(wt) ]; 116 | 117 | 118 | Kj=A*Pj+Pj*A'+ N; 119 | 120 | end 121 | 122 | 123 | 124 | 125 | function [ z ]=specialadd( y , k ) 126 | z=y; 127 | z.J1=y.J1*so3_exp( k(1:3,1) ); 128 | z.J2=y.J2+k(1:3,2); 129 | z.J3=y.J3+k(1:3,3); 130 | 131 | z.j1_w=y.j1_w+k(1:3, 4:6); 132 | z.j2_a=y.j2_a+k(1:3, 7:9); 133 | z.j2_w=y.j2_w+k(1:3, 10:12); 134 | 135 | z.j3_a=y.j3_a+k(1:3, 13:15); 136 | z.j3_w=y.j3_w+k(1:3, 16:18); 137 | 138 | 139 | end 140 | -------------------------------------------------------------------------------- /VINS_DataGenerator/so3_exp.m: -------------------------------------------------------------------------------- 1 | function [ result ] = so3_exp( x ) 2 | % compute the exponential mapping of R^3 to SO(3) 3 | % use sparse matrix 4 | N = size(x,1); 5 | %if N == 3 6 | theta=norm(x); 7 | if theta==0 8 | result=eye(3); 9 | else 10 | omega =x/theta; 11 | result=eye(3,3) + sin(theta) * skew(omega) + (1 - cos(theta))*skew(omega)^2; 12 | end 13 | 14 | %else 15 | % todo, compute exp() of R^6 to SE(3)? 16 | % m = (N-3)/3; 17 | % result=speye(3+m); 18 | % result(1:3,1:3)=Exp( x(1:3)); 19 | %end 20 | end 21 | 22 | -------------------------------------------------------------------------------- /VINS_DataGenerator/testInte.m: -------------------------------------------------------------------------------- 1 | clc 2 | 3 | 4 | load matlab.mat; 5 | load matlab2.mat; 6 | 7 | g = [0;0;-9.8]; dt =0.4; 8 | Pose0 = RobotState{1, 1}.pose; Pose1 = RobotState{1, 2}.pose; 9 | R0 = Pose0(1:3,1:3); P0= Pose0(1:3,4); R1 = Pose1(1:3,1:3); P1= Pose1(1:3,4); 10 | v0 = RobotState{1, 1}.velocity; v1 = RobotState{1, 2}.velocity; 11 | dR = R0'*R1; 12 | dV = R0'*( R1*v1 - R0*v0 - g*dt ); 13 | dP = R0'*( P1- P0 - R0*v0*dt - 0.5*g*dt^2 ); 14 | 15 | 16 | [ value, inf ] = rk4_manifold_mod( M, N , accbias_es, gyrobias_es); 17 | [ value2, inf ] = rk4_manifold_mod2( M, N , accbias_es, gyrobias_es); 18 | [ value3, inf ] = rk4_manifold_mod3( M, N , accbias_es, gyrobias_es); 19 | [ value4, inf ] = rk4_manifold_mod4( M, N , accbias_es, gyrobias_es); 20 | A0 =[dR dV dP] 21 | A1= [value.J1 value.J2 value.J3] 22 | A2= [value2.J1 value2.J2 value2.J3] 23 | A3= [value3.J1 value3.J2 value3.J3] 24 | A4= [value4.J1 value4.J2 value4.J3] 25 | -------------------------------------------------------------------------------- /VINS_cubic.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS_cubic.mat -------------------------------------------------------------------------------- /VINS_cubic2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS_cubic2.mat -------------------------------------------------------------------------------- /VINS_cubic3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/VINS_cubic3.mat -------------------------------------------------------------------------------- /Vision_Example_Small.m: -------------------------------------------------------------------------------- 1 | clc 2 | clear 3 | addpath('./g2o_files/'); 4 | addpath('./auxilliary/') 5 | addpath('./Math/'); 6 | addpath('./Factor/'); 7 | 8 | 9 | pose0=[eye(3) zeros(3,1)]; 10 | pose1=[expm(skew([ 0.1 ;-0.05; 0.2 ])) [ 2; -2 ;-1] ]; 11 | pose2=[expm(skew([ -0.15 ; 0.05; -0.2 ])) [ -1.5; 2.3 ;-1] ]; 12 | 13 | f=cell(1,10); 14 | f{1} = [ 1; 2 ;9 ]; 15 | f{2} = [ -1; 2 ;7]; 16 | f{3} = [ -2; 1 ; 11 ]; 17 | f{4} = [ -1.5; 2.4 ; 7.6 ]; 18 | f{5}= [ 3; 2 ; 9.4 ]; 19 | f{6}= [ -4; 4 ; 14 ]; 20 | f{7}= [ 4; -6 ; 10 ]; 21 | f{8}= [ 5; -2 ; 9.5 ]; 22 | f{9}= [ 0; 0 ; 4 ]; 23 | f{10}= [ 0.2; 0.5 ; 5 ]; 24 | 25 | % generate 10 co-planar points 26 | 27 | 28 | 29 | %%%%%%%%%%%%%%%%%%%%%%%% 30 | 31 | 32 | 33 | 34 | [ Graph ] = InitializeGraph; 35 | 36 | Prior_measure.value=pose0; 37 | Prior_measure.inf=eye(6); 38 | [ Graph ] = AddUnaryEdge ( Graph, 'PriorPose3_Factor', 'Pose3', 'pose0', Prior_measure ); 39 | 40 | 41 | Measure_translation.value=pose1; 42 | Measure_translation.inf=eye(6); 43 | 44 | [ Graph ] = AddNormalEdge( Graph, 'RelativePose3_Factor', 'Pose3', 'pose0', 'Pose3', 'pose1', Measure_translation ); 45 | 46 | 47 | for i=1:10 48 | [ UV_i_0 ] = GenerateUV_randn( pose0, f{i} ); 49 | Measurement_i_0.value = UV_i_0; 50 | Measurement_i_0.inf = eye(2); 51 | [ Graph ] = AddNormalEdge( Graph, 'Vision_Factor', 'Pose3', 'pose0', 'Landmark3', ['landmark' num2str(i)], Measurement_i_0 ); 52 | 53 | 54 | [ UV_i_1 ] = GenerateUV_randn( pose1, f{i} ); 55 | Measurement_i_1.value = UV_i_1; 56 | Measurement_i_1.inf = eye(2); 57 | [ Graph ] = AddNormalEdge( Graph, 'Vision_Factor', 'Pose3', 'pose1', 'Landmark3', ['landmark' num2str(i)], Measurement_i_1 ); 58 | 59 | [ UV_i_2 ] = GenerateUV_randn( pose2, f{i} ); 60 | Measurement_i_2.value = UV_i_2; 61 | Measurement_i_2.inf = eye(2); 62 | [ Graph ] = AddNormalEdge( Graph, 'Vision_Factor', 'Pose3', 'pose2', 'Landmark3', ['landmark' num2str(i)], Measurement_i_2 ); 63 | end 64 | 65 | %%% Set initial guess via ground truth+noise 66 | Graph.Nodes.Pose3.Values.pose0=pose0; 67 | noise1 = [ expm(skew(randn(3,1)*0.0175)) randn(3,1)*0.05]; 68 | Graph.Nodes.Pose3.Values.pose1=se3_group(pose1, noise1 ) ; 69 | noise2 = [ expm(skew(randn(3,1)*0.02)) randn(3,1)*0.2]; 70 | Graph.Nodes.Pose3.Values.pose2=se3_group(pose2, noise2 ) ; 71 | for i=1:10 72 | Graph.Nodes.Landmark3.Values.(['landmark' num2str(i)])=f{i}+randn(3,1); 73 | end 74 | %%% Set initial guess 75 | 76 | 77 | [ Graph ] = PerformGO( Graph ); 78 | -------------------------------------------------------------------------------- /auxilliary/GenerateUV_randn.m: -------------------------------------------------------------------------------- 1 | function [ UV_measurement ] = GenerateUV_randn( pose, landmark ) 2 | 3 | sigma=1/sqrt(2); 4 | R=pose(1:3,1:3); 5 | p=pose(1:3,4); 6 | f=landmark; 7 | 8 | 9 | 10 | Local= R'*( f - p ); 11 | 12 | 13 | fx = 525.0; 14 | fy = 525.0; 15 | cx0 = 639.5; 16 | cy0 = 479.5; 17 | 18 | x= Local(1); 19 | y= Local(2); 20 | z= Local(3); 21 | 22 | K = [ fx 0; 0 fy]; 23 | 24 | noise = sqrt(2)/2* sigma*randn(2,1); 25 | UV_measurement = K*[x/z; y/z]+ [cx0;cy0] + noise; 26 | 27 | end 28 | 29 | -------------------------------------------------------------------------------- /auxilliary/UVD2LocalXYZ.m: -------------------------------------------------------------------------------- 1 | function [ LocalCoordinate ] = UVD2LocalXYZ( uvd ) 2 | 3 | fx = 525.0; 4 | fy = 525.0; 5 | cx0 = 639.5; 6 | cy0 = 479.5; 7 | 8 | K_inv = [ 1/fx 0; 0 1/fy]; 9 | u=uvd(1); v=uvd(2); d=uvd(3); 10 | 11 | z=d; 12 | 13 | xy= K_inv*z*[ u-cx0; v-cy0 ]; 14 | 15 | 16 | 17 | LocalCoordinate=[xy;z]; 18 | 19 | 20 | end 21 | 22 | -------------------------------------------------------------------------------- /doc/Graph-Optimization-Matlab.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/doc/Graph-Optimization-Matlab.pdf -------------------------------------------------------------------------------- /doc/Optimization_on_manifold_v1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UTS-RI/Matlab-Graph-Optimization/8ccb2ef8d919bf491286ffa9705420b875f025d7/doc/Optimization_on_manifold_v1.pdf -------------------------------------------------------------------------------- /g2o_files/AddComplexEdge.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = AddComplexEdge( Graph, EdgeTypeName, NodeArray_TypeIncluded, Measurement ) 2 | 3 | 4 | Measurement.inf_sqrt=(Measurement.inf)^(1/2) ; 5 | dimension_edge = GetEdgeTypeDimension( EdgeTypeName ); 6 | SizeEdges=size(Graph.Edges,2); 7 | ThisEdgeOrder=SizeEdges+1; 8 | Graph.Edges{ThisEdgeOrder}.EdgeType=EdgeTypeName; 9 | Graph.Edges{ThisEdgeOrder}.ErrorVectorIndex= Graph.TotalDimensionOfEdges + [1 : 1: dimension_edge ]'; 10 | Graph.Edges{ThisEdgeOrder}.Measurement=Measurement; 11 | Graph.Edges{ThisEdgeOrder}.OrderInEdges=SizeEdges+1; 12 | 13 | 14 | 15 | CurrentTotalDimensionOfEdges=Graph.TotalDimensionOfEdges; 16 | num_Node=size( NodeArray_TypeIncluded, 1 ); 17 | 18 | num_fixNodes_inthisedge_now = 0; 19 | for i=1: num_Node 20 | 21 | 22 | Node_typename_i=NodeArray_TypeIncluded{i,1}; 23 | Node_id_i=NodeArray_TypeIncluded{i,2}; 24 | [ Graph ] = AddOneNode( Graph, Node_typename_i, Node_id_i ); 25 | Graph.Edges{ThisEdgeOrder}.withNodes.(Node_id_i).NodeTypeName=Node_typename_i; 26 | 27 | field_array_i=fields(Graph.Nodes.(Node_typename_i).Values); 28 | NodeID_order_i= find(strcmp(field_array_i, Node_id_i)); 29 | %NodeID_order_i = NodeID_order_i - Graph.Fixed.(Node_typename_i); %% the order in this edge 30 | 31 | FixedNumberInthisEdgeInThisNodeType = GetFixedNumber(Graph.Fixed.IDname, field_array_i, NodeID_order_i ); 32 | NodeID_order_i = NodeID_order_i - FixedNumberInthisEdgeInThisNodeType; 33 | 34 | %Graph.Edges{ThisEdgeOrder}.withNodes.(Node_id_i).OrderInThisType=NodeID_order_i; 35 | 36 | 37 | 38 | if ~isfield(Graph.Fixed.IDname,Node_id_i) 39 | 40 | dimension_node_i = GetNodeTypeDimension( Node_typename_i ); 41 | 42 | [ RowVecotr_Node_i, ColVector_Node_i ] = GenerateIndexVector( CurrentTotalDimensionOfEdges, dimension_edge, dimension_node_i, NodeID_order_i ); 43 | 44 | if min(RowVecotr_Node_i)<0 || min(ColVector_Node_i)<0 45 | 46 | fprintf('warning! RowVecotr_Node_i or ColVector_Node_i < 0 '); 47 | 48 | end 49 | 50 | 51 | Graph.Nodes.(Node_typename_i).Jacobian.RowVector=[ Graph.Nodes.(Node_typename_i).Jacobian.RowVector; RowVecotr_Node_i]; 52 | Graph.Nodes.(Node_typename_i).Jacobian.ColVector=[Graph.Nodes.(Node_typename_i).Jacobian.ColVector; ColVector_Node_i ]; 53 | num= Graph.Nodes.(Node_typename_i).Jacobian.currentNumber_AllElementsInJacobian; 54 | index_i = num+[1:1: dimension_edge*dimension_node_i ]'; 55 | Graph.Edges{SizeEdges+1}.withNodes.(Node_id_i).JacobianIndex=index_i; 56 | Graph.Nodes.(Node_typename_i).Jacobian.currentNumber_AllElementsInJacobian=Graph.Nodes.(Node_typename_i).Jacobian.currentNumber_AllElementsInJacobian+dimension_edge*dimension_node_i; 57 | 58 | end 59 | end 60 | 61 | 62 | 63 | 64 | Graph.TotalDimensionOfEdges=Graph.TotalDimensionOfEdges+dimension_edge; 65 | 66 | end 67 | 68 | function FixedNumberInthisEdgeInThisNodeType = GetFixedNumber(GraphFixedIDname, field_array_i, NodeID_order_i ) 69 | 70 | if NodeID_order_i == 1 71 | FixedNumberInthisEdgeInThisNodeType = 0; 72 | else 73 | FixedNumberInthisEdgeInThisNodeType = 0; 74 | for j = 1: NodeID_order_i-1 75 | 76 | if isfield( GraphFixedIDname, field_array_i{j}) 77 | FixedNumberInthisEdgeInThisNodeType = FixedNumberInthisEdgeInThisNodeType+1; 78 | end 79 | end 80 | end 81 | 82 | end 83 | 84 | 85 | -------------------------------------------------------------------------------- /g2o_files/AddNormalEdge.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = AddNormalEdge( Graph, EdgeTypeName, NodeTypeName_A, NodeID_A, NodeTypeName_B, NodeID_B , Measurement ) 2 | Measurement.inf_sqrt=(Measurement.inf)^(1/2) ; 3 | 4 | dimension_edge = GetEdgeTypeDimension( EdgeTypeName ); 5 | 6 | SizeEdges=size(Graph.Edges,2); 7 | ThisEdgeOrder=SizeEdges+1; 8 | Graph.Edges{ThisEdgeOrder}.EdgeType=EdgeTypeName; 9 | Graph.Edges{ThisEdgeOrder}.withNodes.(NodeID_A).NodeTypeName=NodeTypeName_A; 10 | Graph.Edges{ThisEdgeOrder}.withNodes.(NodeID_B).NodeTypeName=NodeTypeName_B; 11 | Graph.Edges{ThisEdgeOrder}.ErrorVectorIndex= Graph.TotalDimensionOfEdges + [1 : 1: dimension_edge ]'; 12 | 13 | 14 | [ Graph ] = AddOneNode( Graph, NodeTypeName_A, NodeID_A ); 15 | field_array_A=fields(Graph.Nodes.(NodeTypeName_A).Values); 16 | NodeID_order_A= find(strcmp(field_array_A, NodeID_A)); 17 | Graph.Edges{ThisEdgeOrder}.withNodes.(NodeID_A).OrderInThisType=NodeID_order_A; 18 | 19 | [ Graph ] = AddOneNode( Graph, NodeTypeName_B, NodeID_B ); 20 | field_array_B=fields(Graph.Nodes.(NodeTypeName_B).Values); 21 | NodeID_order_B= find(strcmp(field_array_B, NodeID_B)); 22 | Graph.Edges{ThisEdgeOrder}.withNodes.(NodeID_B).OrderInThisType=NodeID_order_B; 23 | 24 | 25 | 26 | Graph.Edges{ThisEdgeOrder}.Measurement=Measurement; 27 | Graph.Edges{ThisEdgeOrder}.OrderInEdges=SizeEdges+1; 28 | 29 | 30 | 31 | dimension_node_A = GetNodeTypeDimension( NodeTypeName_A ); 32 | dimension_node_B = GetNodeTypeDimension( NodeTypeName_B ); 33 | 34 | 35 | CurrentTotalDimensionOfEdges=Graph.TotalDimensionOfEdges; 36 | 37 | [ RowVecotr_Node_A, ColVector_Node_A ] = GenerateIndexVector( CurrentTotalDimensionOfEdges, dimension_edge, dimension_node_A, NodeID_order_A ); 38 | Graph.Nodes.(NodeTypeName_A).Jacobian.RowVector=[ Graph.Nodes.(NodeTypeName_A).Jacobian.RowVector; RowVecotr_Node_A]; 39 | Graph.Nodes.(NodeTypeName_A).Jacobian.ColVector=[Graph.Nodes.(NodeTypeName_A).Jacobian.ColVector; ColVector_Node_A ]; 40 | num= Graph.Nodes.(NodeTypeName_A).Jacobian.currentNumber_AllElementsInJacobian; 41 | index_A= num+[1:1: dimension_edge*dimension_node_A ]'; 42 | %Graph.Edges{SizeEdges+1}.Jacobian.(NodeTypeName_A).JacobianIndex=index_A; 43 | %%% 44 | Graph.Edges{SizeEdges+1}.withNodes.(NodeID_A).JacobianIndex=index_A; 45 | 46 | Graph.Nodes.(NodeTypeName_A).Jacobian.currentNumber_AllElementsInJacobian=Graph.Nodes.(NodeTypeName_A).Jacobian.currentNumber_AllElementsInJacobian+dimension_edge*dimension_node_A; 47 | 48 | 49 | 50 | 51 | [ RowVecotr_Node_B, ColVector_Node_B ] = GenerateIndexVector( CurrentTotalDimensionOfEdges, dimension_edge, dimension_node_B, NodeID_order_B ); 52 | Graph.Nodes.(NodeTypeName_B).Jacobian.RowVector=[ Graph.Nodes.(NodeTypeName_B).Jacobian.RowVector; RowVecotr_Node_B]; 53 | Graph.Nodes.(NodeTypeName_B).Jacobian.ColVector=[Graph.Nodes.(NodeTypeName_B).Jacobian.ColVector; ColVector_Node_B ]; 54 | num= Graph.Nodes.(NodeTypeName_B).Jacobian.currentNumber_AllElementsInJacobian; 55 | index_B= num+[1:1: dimension_edge*dimension_node_B ]'; 56 | 57 | Graph.Edges{SizeEdges+1}.withNodes.(NodeID_B).JacobianIndex=index_B; %%% add 58 | 59 | Graph.Nodes.(NodeTypeName_B).Jacobian.currentNumber_AllElementsInJacobian=Graph.Nodes.(NodeTypeName_B).Jacobian.currentNumber_AllElementsInJacobian+dimension_edge*dimension_node_B; 60 | 61 | 62 | 63 | 64 | Graph.TotalDimensionOfEdges=Graph.TotalDimensionOfEdges+dimension_edge; 65 | 66 | 67 | end 68 | 69 | -------------------------------------------------------------------------------- /g2o_files/AddOneNode.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = AddOneNode( Graph, NodeTypeName, NodeID ) 2 | 3 | 4 | 5 | if isfield(Graph.Nodes, NodeTypeName) 6 | if isfield(Graph.Nodes.(NodeTypeName).Values, NodeID) 7 | else 8 | Graph.Nodes.(NodeTypeName).Values.(NodeID) = SetNodeDefaultValue( NodeTypeName ); 9 | 10 | if isfield(Graph.Fixed.IDname, NodeID) 11 | 12 | Graph.Fixed.(NodeTypeName)= Graph.Fixed.(NodeTypeName)+1; 13 | end 14 | 15 | end 16 | else 17 | Graph.Nodes.(NodeTypeName).Dimension=GetNodeTypeDimension( NodeTypeName ); 18 | Graph.Nodes.(NodeTypeName).Jacobian.currentNumber_AllElementsInJacobian=0; 19 | Graph.Nodes.(NodeTypeName).Jacobian.ValueVector=[]; 20 | Graph.Nodes.(NodeTypeName).Jacobian.RowVector=[]; 21 | Graph.Nodes.(NodeTypeName).Jacobian.ColVector=[]; 22 | Graph.Nodes.(NodeTypeName).Values.(NodeID) = SetNodeDefaultValue( NodeTypeName ); 23 | Graph.Fixed.(NodeTypeName)= 0; 24 | 25 | if isfield(Graph.Fixed.IDname, NodeID) 26 | Graph.Fixed.(NodeTypeName)= Graph.Fixed.(NodeTypeName)+1; 27 | end 28 | end 29 | 30 | 31 | end 32 | 33 | -------------------------------------------------------------------------------- /g2o_files/AddUnaryEdge.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = AddUnaryEdge( Graph, EdgeTypeName, NodeTypeName, NodeID, Measurement ) 2 | Measurement.inf_sqrt=(Measurement.inf)^(1/2) ; 3 | 4 | dimension_edge = GetEdgeTypeDimension( EdgeTypeName ); 5 | 6 | 7 | 8 | 9 | SizeEdges=size(Graph.Edges,2); 10 | ThisEdgeOrder=SizeEdges+1; 11 | Graph.Edges{ThisEdgeOrder}.EdgeType=EdgeTypeName; 12 | Graph.Edges{ThisEdgeOrder}.withNodes.(NodeID).NodeTypeName=NodeTypeName; 13 | Graph.Edges{ThisEdgeOrder}.ErrorVectorIndex= Graph.TotalDimensionOfEdges + [1 : 1: dimension_edge ]'; 14 | 15 | 16 | 17 | [ Graph ] = AddOneNode( Graph, NodeTypeName, NodeID ); 18 | field_array=fields(Graph.Nodes.(NodeTypeName).Values); 19 | NodeID_order= find(strcmp(field_array, NodeID)); 20 | Graph.Edges{ThisEdgeOrder}.withNodes.(NodeID).OrderInThisType=NodeID_order; 21 | 22 | 23 | Graph.Edges{ThisEdgeOrder}.Measurement=Measurement; 24 | Graph.Edges{ThisEdgeOrder}.OrderInEdges=SizeEdges+1; 25 | 26 | dimension_node = GetNodeTypeDimension( NodeTypeName ); 27 | 28 | 29 | CurrentTotalDimensionOfEdges=Graph.TotalDimensionOfEdges; 30 | [ RowVecotr_ThisNode, ColVector_ThisNode ] = GenerateIndexVector( CurrentTotalDimensionOfEdges, dimension_edge, dimension_node, NodeID_order ); 31 | Graph.Nodes.(NodeTypeName).Jacobian.RowVector=[ Graph.Nodes.(NodeTypeName).Jacobian.RowVector; RowVecotr_ThisNode]; 32 | Graph.Nodes.(NodeTypeName).Jacobian.ColVector=[Graph.Nodes.(NodeTypeName).Jacobian.ColVector; ColVector_ThisNode ]; 33 | num= Graph.Nodes.(NodeTypeName).Jacobian.currentNumber_AllElementsInJacobian; 34 | index= num+[1:1: dimension_edge*dimension_node ]'; 35 | %Graph.Edges{SizeEdges+1}.Jacobian.(NodeTypeName).JacobianIndex=index; 36 | 37 | % 38 | Graph.Edges{SizeEdges+1}.withNodes.(NodeID).JacobianIndex=index; 39 | 40 | Graph.Nodes.(NodeTypeName).Jacobian.currentNumber_AllElementsInJacobian=Graph.Nodes.(NodeTypeName).Jacobian.currentNumber_AllElementsInJacobian+dimension_edge*dimension_node; 41 | Graph.TotalDimensionOfEdges=Graph.TotalDimensionOfEdges+dimension_edge; 42 | 43 | 44 | 45 | 46 | end 47 | 48 | -------------------------------------------------------------------------------- /g2o_files/Cal_ObjValue.m: -------------------------------------------------------------------------------- 1 | function [ Objvalue ] = Cal_ObjValue( Graph ) 2 | Objvalue= 0; 3 | NumberofEdges=size(Graph.Edges, 2); 4 | for iter_edge=1:NumberofEdges 5 | edge =Graph.Edges{iter_edge}; 6 | edgeType=edge.EdgeType; 7 | num_nodes_ThisEdge= size( fieldnames(edge.withNodes),1); 8 | Nodes_array=cell(num_nodes_ThisEdge, 1 ); 9 | nodes_id = fields(edge.withNodes); 10 | for i = 1: num_nodes_ThisEdge 11 | node_id = nodes_id{i}; 12 | node_type= edge.withNodes.(node_id).NodeTypeName; 13 | Nodes_array{i}=Graph.Nodes.(node_type).Values.(node_id); 14 | end 15 | FuncFactor=str2func(edgeType); 16 | [ ErrorVector, ~ ]=FuncFactor( Nodes_array, edge.Measurement.value ); 17 | Objvalue = Objvalue + ErrorVector'*edge.Measurement.inf*ErrorVector; 18 | end 19 | 20 | % Total_ErrorVector=zeros( Graph.TotalDimensionOfEdges, 1 ); %%%%% Initialize the Error Vector 21 | % Number_NodeTypes=size( fields(Graph.Nodes) ,1 ); 22 | % Total_Jacobian=struct; 23 | % NodeTypeNamesArray = fields(Graph.Nodes); 24 | % 25 | % num_update=0; 26 | % for i=1:Number_NodeTypes 27 | % Total_Jacobian.( NodeTypeNamesArray{i} )=[]; 28 | % dimension= Graph.Nodes.( NodeTypeNamesArray{i} ).Dimension; 29 | % num_node_thisKind = size( fields (Graph.Nodes.( NodeTypeNamesArray{i} ).Values) , 1) ; 30 | % 31 | % if isfield(Graph.Fixed, NodeTypeNamesArray{i}) 32 | % num_node_thisKind = num_node_thisKind - Graph.Fixed.(NodeTypeNamesArray{i}); 33 | % end 34 | % 35 | % Graph.Nodes.( NodeTypeNamesArray{i} ).updateIndex = num_update+ [1:1: dimension* num_node_thisKind ]'; 36 | % num_update=num_update+ dimension* num_node_thisKind; 37 | % end 38 | % 39 | % NumberofEdges=size(Graph.Edges, 2); 40 | % 41 | % 42 | % for iter_edge=1:NumberofEdges 43 | % 44 | % edge =Graph.Edges{iter_edge}; 45 | % edgeType=edge.EdgeType; 46 | % 47 | % num_nodes_ThisEdge= size( fieldnames(edge.withNodes),1); 48 | % Nodes_array=cell(num_nodes_ThisEdge, 1 ); 49 | % nodes_id = fields(edge.withNodes); 50 | % for i = 1: num_nodes_ThisEdge 51 | % node_id = nodes_id{i}; 52 | % node_type= edge.withNodes.(node_id).NodeTypeName; 53 | % Nodes_array{i}=Graph.Nodes.(node_type).Values.(node_id); 54 | % end 55 | % FuncFactor=str2func(edgeType); 56 | % [ ErrorVector, ~ ]=FuncFactor( Nodes_array, edge.Measurement.value ); 57 | % 58 | % Total_ErrorVector( edge.ErrorVectorIndex )= edge.Measurement.inf_sqrt* ErrorVector; 59 | % 60 | % end 61 | % 62 | % Objvalue = Total_ErrorVector'*Total_ErrorVector; 63 | 64 | end 65 | 66 | -------------------------------------------------------------------------------- /g2o_files/Cal_ObjValue_New.m: -------------------------------------------------------------------------------- 1 | function [ Objvalue ] = Cal_ObjValue_New( Graph ) 2 | Objvalue= 0; 3 | NumberofEdges=size(Graph.Edges, 2); 4 | for iter_edge=1:NumberofEdges 5 | edge =Graph.Edges{iter_edge}; 6 | edgeType=edge.EdgeType; 7 | num_nodes_ThisEdge= size( fieldnames(edge.withNodes),1); 8 | Nodes_array=cell(num_nodes_ThisEdge, 1 ); 9 | nodes_id = fields(edge.withNodes); 10 | for i = 1: num_nodes_ThisEdge 11 | node_id = nodes_id{i}; 12 | node_type= edge.withNodes.(node_id).NodeTypeName; 13 | Nodes_array{i}=Graph.Nodes.(node_type).Values.(node_id); 14 | end 15 | FuncFactor=str2func(edgeType); 16 | [ ErrorVector, ~ ]=FuncFactor( Nodes_array, edge.Measurement.value ); 17 | Objvalue = Objvalue + ErrorVector'*edge.Measurement.inf*ErrorVector; 18 | end 19 | 20 | 21 | end 22 | 23 | -------------------------------------------------------------------------------- /g2o_files/Fn_processNodeTable.m: -------------------------------------------------------------------------------- 1 | function NodeTable = Fn_processNodeTable( NodeTable ) 2 | types = fields(NodeTable.schur); 3 | num_types = size(types, 1); 4 | 5 | a = 0; 6 | for i=1:num_types 7 | 8 | type_i = types{i}; 9 | dimension = NodeTable.schur.(type_i).dimension; 10 | upindex = NodeTable.schur.(type_i).updateIndex; 11 | num_i = (upindex(end)+1 - upindex(1) )/dimension; 12 | a = a+ num_i*dimension^2; 13 | NodeTable.schur.(type_i).num = a; 14 | 15 | end 16 | 17 | 18 | 19 | end 20 | -------------------------------------------------------------------------------- /g2o_files/GenerateIndexVector.m: -------------------------------------------------------------------------------- 1 | function [ RowVecotr_ThisNode, ColVector_ThisNode ] = GenerateIndexVector( CurrentTotalDimensionOfEdges, dimension_edge, dimension_node, NodeID_order ) 2 | 3 | 4 | A= 1:1:dimension_edge; 5 | A= A'; 6 | 7 | B= 1:1:dimension_node; 8 | 9 | RowMatrix= repmat(A, 1, dimension_node )+ CurrentTotalDimensionOfEdges ; 10 | VectorMatrix = repmat( B, dimension_edge,1 ) + (NodeID_order-1)*dimension_node; 11 | 12 | 13 | RowVecotr_ThisNode=RowMatrix(:); 14 | ColVector_ThisNode=VectorMatrix(:); 15 | 16 | end 17 | 18 | -------------------------------------------------------------------------------- /g2o_files/GetInformationMatrix.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = GetInformationMatrix( Graph, TypeName_this ) 2 | %%%%%%%%%%%%%% generate the sparse Jaocbian matrix and the error vecotr 3 | Total_ErrorVector=zeros( Graph.TotalDimensionOfEdges, 1 ); %%%%% Initialize the Error Vector 4 | Number_NodeTypes=size( fields(Graph.Nodes) ,1 ); 5 | Total_Jacobian=struct; 6 | NodeTypeNamesArray = fields(Graph.Nodes); 7 | 8 | num_update=0; 9 | for i=1:Number_NodeTypes 10 | Total_Jacobian.( NodeTypeNamesArray{i} )=[]; 11 | dimension= Graph.Nodes.( NodeTypeNamesArray{i} ).Dimension; 12 | num_node_thisKind = size( fields (Graph.Nodes.( NodeTypeNamesArray{i} ).Values) , 1) ; 13 | Graph.Nodes.( NodeTypeNamesArray{i} ).updateIndex = num_update+ [1:1: dimension* num_node_thisKind ]'; 14 | num_update=num_update+ dimension* num_node_thisKind; 15 | end 16 | 17 | NumberofEdges=size(Graph.Edges, 2); 18 | NeedOptimization=true; 19 | iter_optimization=0; 20 | 21 | while ( NeedOptimization && iter_optimization < Graph.Parameters.iter ) 22 | for iter_edge=1:NumberofEdges 23 | NeedOptimization=false; 24 | 25 | 26 | edge =Graph.Edges{iter_edge}; 27 | edgeType=edge.EdgeType; 28 | 29 | 30 | 31 | 32 | num_nodes_ThisEdge= size( fieldnames(edge.withNodes),1); 33 | Nodes_array=cell(num_nodes_ThisEdge, 1 ); 34 | nodes_id = fields(edge.withNodes); 35 | for i = 1: num_nodes_ThisEdge 36 | node_id = nodes_id{i}; 37 | node_type= edge.withNodes.(node_id).NodeTypeName; 38 | Nodes_array{i}=Graph.Nodes.(node_type).Values.(node_id); 39 | end 40 | FuncFactor=str2func(edgeType); 41 | [ ErrorVector, Jacobian_Node ]=FuncFactor( Nodes_array, edge.Measurement.value ); 42 | 43 | Total_ErrorVector( edge.ErrorVectorIndex )= edge.Measurement.inf_sqrt* ErrorVector; 44 | 45 | %% debug 46 | error_s = edge.Measurement.inf_sqrt* ErrorVector; 47 | Error_s = error_s'*error_s; 48 | 49 | 50 | for i = 1: num_nodes_ThisEdge 51 | node_id = nodes_id{i}; 52 | node_type= edge.withNodes.(node_id).NodeTypeName; 53 | 54 | Jacobian_Node_i_infsqrt=edge.Measurement.inf_sqrt*Jacobian_Node{i}; 55 | Jacobianindex_thisEdge_thisNode=edge.withNodes.(node_id).JacobianIndex; 56 | Graph.Nodes.(node_type).Jacobian.ValueVector( Jacobianindex_thisEdge_thisNode )=Jacobian_Node_i_infsqrt(:); 57 | 58 | end 59 | 60 | end 61 | 62 | 63 | sprintf('cost = %f\n',Total_ErrorVector'*Total_ErrorVector ) 64 | 65 | 66 | 67 | Whole_Jacobian=[]; 68 | for i=1:Number_NodeTypes 69 | ThisTypeName=NodeTypeNamesArray{i}; 70 | all_ids_thisType= fields( Graph.Nodes.( ThisTypeName ).Values ); 71 | num_ids_thisType= size ( all_ids_thisType, 1 ); 72 | num_cols=num_ids_thisType*Graph.Nodes.(ThisTypeName).Dimension; 73 | num_rows=Graph.TotalDimensionOfEdges; 74 | 75 | Total_Jacobian.( NodeTypeNamesArray{i} )=sparse( Graph.Nodes.(ThisTypeName).Jacobian.RowVector, Graph.Nodes.(ThisTypeName).Jacobian.ColVector, Graph.Nodes.(ThisTypeName).Jacobian.ValueVector, num_rows, num_cols ); 76 | Whole_Jacobian=[Whole_Jacobian Total_Jacobian.( NodeTypeNamesArray{i} )]; 77 | end 78 | 79 | 80 | 81 | 82 | if strcmp(Graph.Parameters.OptimizationMethod,'GN') 83 | dX= - (Whole_Jacobian'*Whole_Jacobian)\(Whole_Jacobian'*Total_ErrorVector); % this is the incremental vector 84 | else 85 | H = Whole_Jacobian'*Whole_Jacobian; mm=size(H,1); 86 | %lambda = 0.2; 87 | dX = (speye(mm)*lambda-H)\(Whole_Jacobian'*Total_ErrorVector); 88 | end 89 | 90 | 91 | 92 | 93 | 94 | %%%%% update_state 95 | for i=1:Number_NodeTypes 96 | ThisTypeName=NodeTypeNamesArray{i}; 97 | dx = dX (Graph.Nodes.( ThisTypeName ).updateIndex); 98 | dimension_thisType= Graph.Nodes.( ThisTypeName ).Dimension; 99 | 100 | all_ids_thisType= fields( Graph.Nodes.( ThisTypeName ).Values ); 101 | num_ids_thisType= size ( all_ids_thisType, 1 ); 102 | 103 | for k=1:num_ids_thisType 104 | thisNode_id = all_ids_thisType{k}; 105 | increment = dx ( dimension_thisType*(k-1)+1 : dimension_thisType*k ); 106 | 107 | Z= abs( increment )-Graph.Parameters.LinearizedError.(ThisTypeName); 108 | if any(Z>0) 109 | NeedOptimization=true; 110 | end 111 | 112 | Graph.Nodes.( ThisTypeName ).Values.( thisNode_id ) = update_state( ThisTypeName, Graph.Nodes.( ThisTypeName ).Values.( thisNode_id ) , increment); 113 | end 114 | 115 | end 116 | 117 | iter_optimization=iter_optimization+1; 118 | end 119 | 120 | 121 | end 122 | -------------------------------------------------------------------------------- /g2o_files/InitializeGraph.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = InitializeGraph 2 | 3 | 4 | 5 | Graph.Parameters.OptimizationMethod='GN'; 6 | Graph.Parameters.LinearizedError.Pose3 = [0.0001; 0.0001; 0.0001; 0.001; 0.001; 0.001 ] ; 7 | Graph.Parameters.LinearizedError.LPose3 = [0.0001; 0.0001; 0.0001; 0.001; 0.001; 0.001 ] ; 8 | 9 | 10 | 11 | Graph.Parameters.LinearizedError.Velocity3 = [0.001; 0.001; 0.001 ] ; 12 | Graph.Parameters.LinearizedError.IMUbias = [0.0001; 0.0001; 0.0001; 0.001; 0.001; 0.001 ] ; 13 | 14 | 15 | Graph.Parameters.LinearizedError.Landmark3= [0.01; 0.01 ; 0.01 ]; 16 | 17 | Graph.Parameters.LinearizedError.Pose2 = [0.0001; 0.001; 0.001 ] ; 18 | Graph.Parameters.LinearizedError.Landmark2= [0.001; 0.001 ]; 19 | Graph.Parameters.LinearizedError.ParallaxPoint= [0.0001; 0.0001;0.0001 ]; 20 | 21 | 22 | 23 | Graph.Parameters.tau = 1e-5; 24 | Graph.Parameters.eplison = 1e-3; 25 | 26 | Graph.Parameters.eplison_1 = 1e-4; 27 | Graph.Parameters.eplison_2 = 1e-4; 28 | Graph.Parameters.eplison_3 = 1e-4; 29 | Graph.Parameters.delta = 1; 30 | 31 | 32 | Graph.Parameters.iter= 250; 33 | 34 | Graph.Fixed=[]; 35 | Graph.Fixed.IDname=[]; 36 | 37 | Graph.Nodes=[]; 38 | Graph.Edges=[]; 39 | Graph.TotalDimensionOfEdges=0; 40 | 41 | 42 | 43 | 44 | 45 | end 46 | 47 | -------------------------------------------------------------------------------- /g2o_files/PerformGO.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = PerformGO( Graph ) 2 | %%%%%%%%%%%%%% generate the sparse Jaocbian matrix and the error vecotr 3 | Total_ErrorVector=zeros( Graph.TotalDimensionOfEdges, 1 ); %%%%% Initialize the Error Vector 4 | Number_NodeTypes=size( fields(Graph.Nodes) ,1 ); 5 | Total_Jacobian=struct; 6 | NodeTypeNamesArray = fields(Graph.Nodes); 7 | 8 | num_update=0; 9 | for i=1:Number_NodeTypes 10 | Total_Jacobian.( NodeTypeNamesArray{i} )=[]; 11 | dimension= Graph.Nodes.( NodeTypeNamesArray{i} ).Dimension; 12 | num_node_thisKind = size( fields (Graph.Nodes.( NodeTypeNamesArray{i} ).Values) , 1) ; 13 | 14 | if isfield(Graph.Fixed, NodeTypeNamesArray{i}) 15 | num_node_thisKind = num_node_thisKind - Graph.Fixed.(NodeTypeNamesArray{i}); 16 | end 17 | 18 | Graph.Nodes.( NodeTypeNamesArray{i} ).updateIndex = num_update+ [1:1: dimension* num_node_thisKind ]'; 19 | num_update=num_update+ dimension* num_node_thisKind; 20 | end 21 | 22 | NumberofEdges=size(Graph.Edges, 2); 23 | NeedOptimization=true; 24 | iter_optimization=0; 25 | 26 | while ( NeedOptimization && iter_optimization < Graph.Parameters.iter ) 27 | for iter_edge=1:NumberofEdges 28 | NeedOptimization=false; 29 | 30 | 31 | edge =Graph.Edges{iter_edge}; 32 | edgeType=edge.EdgeType; 33 | 34 | 35 | 36 | 37 | num_nodes_ThisEdge= size( fieldnames(edge.withNodes),1); 38 | Nodes_array=cell(num_nodes_ThisEdge, 1 ); 39 | nodes_id = fields(edge.withNodes); 40 | for i = 1: num_nodes_ThisEdge 41 | node_id = nodes_id{i}; 42 | node_type= edge.withNodes.(node_id).NodeTypeName; 43 | Nodes_array{i}=Graph.Nodes.(node_type).Values.(node_id); 44 | end 45 | FuncFactor=str2func(edgeType); 46 | temp_value = edge.Measurement.value; 47 | [ ErrorVector, Jacobian_Node ]=FuncFactor( Nodes_array, temp_value ); 48 | 49 | Total_ErrorVector( edge.ErrorVectorIndex )= edge.Measurement.inf_sqrt* ErrorVector; 50 | 51 | %% debug 52 | error_s = edge.Measurement.inf_sqrt* ErrorVector; 53 | Error_s = error_s'*error_s; 54 | 55 | 56 | for i = 1: num_nodes_ThisEdge 57 | node_id = nodes_id{i}; 58 | node_type= edge.withNodes.(node_id).NodeTypeName; 59 | 60 | if ~isfield( Graph.Fixed.IDname, node_id) 61 | Jacobian_Node_i_infsqrt=edge.Measurement.inf_sqrt*Jacobian_Node{i}; 62 | Jacobianindex_thisEdge_thisNode=edge.withNodes.(node_id).JacobianIndex; 63 | Graph.Nodes.(node_type).Jacobian.ValueVector( Jacobianindex_thisEdge_thisNode )=Jacobian_Node_i_infsqrt(:); 64 | end 65 | 66 | end 67 | 68 | end 69 | 70 | 71 | sprintf('cost = %f\n',Total_ErrorVector'*Total_ErrorVector ) 72 | 73 | 74 | 75 | Whole_Jacobian=[]; 76 | for i=1:Number_NodeTypes 77 | ThisTypeName=NodeTypeNamesArray{i}; 78 | all_ids_thisType= fields( Graph.Nodes.( ThisTypeName ).Values ); 79 | num_ids_thisType= size ( all_ids_thisType, 1 ); 80 | 81 | num_ids_thisType = num_ids_thisType - Graph.Fixed.(ThisTypeName); % new 82 | 83 | num_cols=num_ids_thisType*Graph.Nodes.(ThisTypeName).Dimension; 84 | num_rows=Graph.TotalDimensionOfEdges; 85 | 86 | 87 | 88 | Total_Jacobian.( NodeTypeNamesArray{i} )=sparse( Graph.Nodes.(ThisTypeName).Jacobian.RowVector, Graph.Nodes.(ThisTypeName).Jacobian.ColVector, Graph.Nodes.(ThisTypeName).Jacobian.ValueVector, num_rows, num_cols ); 89 | Whole_Jacobian=[Whole_Jacobian Total_Jacobian.( NodeTypeNamesArray{i} )]; 90 | end 91 | 92 | 93 | 94 | 95 | if strcmp(Graph.Parameters.OptimizationMethod,'GN') 96 | dX= - (Whole_Jacobian'*Whole_Jacobian)\(Whole_Jacobian'*Total_ErrorVector); % this is the incremental vector 97 | else 98 | H = Whole_Jacobian'*Whole_Jacobian; mm=size(H,1); 99 | %lambda = 0.2; 100 | dX = (speye(mm)*lambda-H)\(Whole_Jacobian'*Total_ErrorVector); 101 | end 102 | 103 | 104 | 105 | 106 | 107 | %%%%% update_state 108 | for i=1:Number_NodeTypes 109 | ThisTypeName=NodeTypeNamesArray{i}; 110 | dx = dX (Graph.Nodes.( ThisTypeName ).updateIndex); 111 | dimension_thisType= Graph.Nodes.( ThisTypeName ).Dimension; 112 | 113 | all_ids_thisType= fields( Graph.Nodes.( ThisTypeName ).Values ); 114 | num_ids_thisType= size ( all_ids_thisType, 1 ); 115 | 116 | PreFixed_num = 0; 117 | for k=1:num_ids_thisType 118 | thisNode_id = all_ids_thisType{k}; 119 | 120 | if ~isfield(Graph.Fixed.IDname, thisNode_id) 121 | %%increment = dx ( dimension_thisType*(k-1)+1 : dimension_thisType*k ); %%% 122 | increment = dx ( dimension_thisType*(k-1-PreFixed_num)+1 : dimension_thisType*(k-PreFixed_num) ); 123 | 124 | Z= abs( increment )-Graph.Parameters.LinearizedError.(ThisTypeName); 125 | if any(Z>0) 126 | NeedOptimization=true; 127 | end 128 | 129 | Graph.Nodes.( ThisTypeName ).Values.( thisNode_id ) = update_state( ThisTypeName, Graph.Nodes.( ThisTypeName ).Values.( thisNode_id ) , increment); 130 | else 131 | PreFixed_num = PreFixed_num+1; 132 | end 133 | end 134 | 135 | end 136 | 137 | iter_optimization=iter_optimization+1; 138 | end 139 | 140 | 141 | end 142 | -------------------------------------------------------------------------------- /g2o_files/PerformGO_DLBackup.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = PerformGO_DLBackup( Graph ) 2 | %%%%%%%%%%%%%% generate the sparse Jaocbian matrix and the error vecotr 3 | Total_ErrorVector=zeros( Graph.TotalDimensionOfEdges, 1 ); %%%%% Initialize the Error Vector 4 | Number_NodeTypes=size( fields(Graph.Nodes) ,1 ); 5 | Total_Jacobian=struct; 6 | NodeTypeNamesArray = fields(Graph.Nodes); 7 | 8 | num_update=0; 9 | for i=1:Number_NodeTypes 10 | Total_Jacobian.( NodeTypeNamesArray{i} )=[]; 11 | dimension= Graph.Nodes.( NodeTypeNamesArray{i} ).Dimension; 12 | num_node_thisKind = size( fields (Graph.Nodes.( NodeTypeNamesArray{i} ).Values) , 1) ; 13 | Graph.Nodes.( NodeTypeNamesArray{i} ).updateIndex = num_update+ [1:1: dimension* num_node_thisKind ]'; 14 | num_update=num_update+ dimension* num_node_thisKind; 15 | end 16 | 17 | NumberofEdges=size(Graph.Edges, 2); 18 | iter_optimization=0; 19 | 20 | eplison_1= Graph.Parameters.eplison_1; 21 | eplison_2= Graph.Parameters.eplison_2; 22 | eplison_3= Graph.Parameters.eplison_3; 23 | delta = Graph.Parameters.delta; 24 | 25 | Graph.Edge_Error = zeros( NumberofEdges,1 ); 26 | 27 | stop = 0; 28 | while ( ~stop && iter_optimization < Graph.Parameters.iter ) 29 | 30 | for iter_edge=1:NumberofEdges 31 | edge =Graph.Edges{iter_edge}; 32 | edgeType=edge.EdgeType; 33 | 34 | num_nodes_ThisEdge= size( fieldnames(edge.withNodes),1); 35 | Nodes_array=cell(num_nodes_ThisEdge, 1 ); 36 | nodes_id = fields(edge.withNodes); 37 | for i = 1: num_nodes_ThisEdge 38 | node_id = nodes_id{i}; 39 | node_type= edge.withNodes.(node_id).NodeTypeName; 40 | Nodes_array{i}=Graph.Nodes.(node_type).Values.(node_id); 41 | end 42 | FuncFactor=str2func(edgeType); 43 | [ ErrorVector, Jacobian_Node ]=FuncFactor( Nodes_array, edge.Measurement.value ); 44 | 45 | Total_ErrorVector( edge.ErrorVectorIndex )= edge.Measurement.inf_sqrt* ErrorVector; 46 | Graph.Edge_Error( iter_edge ) = (edge.Measurement.inf_sqrt* ErrorVector)'*(edge.Measurement.inf_sqrt* ErrorVector); 47 | 48 | for i = 1: num_nodes_ThisEdge 49 | node_id = nodes_id{i}; 50 | node_type= edge.withNodes.(node_id).NodeTypeName; 51 | 52 | Jacobian_Node_i_infsqrt=edge.Measurement.inf_sqrt*Jacobian_Node{i}; 53 | Jacobianindex_thisEdge_thisNode=edge.withNodes.(node_id).JacobianIndex; 54 | Graph.Nodes.(node_type).Jacobian.ValueVector( Jacobianindex_thisEdge_thisNode )=Jacobian_Node_i_infsqrt(:); 55 | 56 | end 57 | 58 | end 59 | 60 | F_0 = Total_ErrorVector'*Total_ErrorVector; 61 | L_0 = F_0; 62 | sprintf('cost = %f\n',Total_ErrorVector'*Total_ErrorVector ) 63 | 64 | 65 | 66 | Whole_Jacobian=[]; 67 | for i=1:Number_NodeTypes 68 | ThisTypeName=NodeTypeNamesArray{i}; 69 | all_ids_thisType= fields( Graph.Nodes.( ThisTypeName ).Values ); 70 | num_ids_thisType= size ( all_ids_thisType, 1 ); 71 | num_cols=num_ids_thisType*Graph.Nodes.(ThisTypeName).Dimension; 72 | num_rows=Graph.TotalDimensionOfEdges; 73 | 74 | Total_Jacobian.( NodeTypeNamesArray{i} )=sparse( Graph.Nodes.(ThisTypeName).Jacobian.RowVector, Graph.Nodes.(ThisTypeName).Jacobian.ColVector, Graph.Nodes.(ThisTypeName).Jacobian.ValueVector, num_rows, num_cols ); 75 | Whole_Jacobian=[Whole_Jacobian Total_Jacobian.( NodeTypeNamesArray{i} )]; 76 | end 77 | 78 | 79 | 80 | g = Whole_Jacobian'*Total_ErrorVector; 81 | HH = Whole_Jacobian'*Whole_Jacobian; 82 | h_gn = -HH\(Whole_Jacobian'*Total_ErrorVector); 83 | alpha = (g'*g)/(g'*HH*g ); 84 | h_sd = -alpha*g; 85 | h_dl = compute_DogLeg( h_gn, h_sd , delta ); 86 | rasidual = norm(Total_ErrorVector, inf); 87 | g_inf = norm(g, inf); 88 | stop = (norm( h_dl, 2 ) <= eplison_2); 89 | 90 | 91 | dX = h_dl; 92 | 93 | 94 | L_new = (Total_ErrorVector+Whole_Jacobian*dX)'*(Total_ErrorVector+Whole_Jacobian*dX); 95 | 96 | previousNodes = Graph.Nodes; 97 | 98 | %%%%%%% Test 99 | 100 | % Jv = Total_Jacobian.Velocity3; 101 | % Hv = Jv'*Jv; 102 | % Hv0 = Hv(1:3,1:3); 103 | % %fprintf( 'cond is %f\n', cond(HH(11:end,11:end)) ) 104 | % SS = inv(HH); 105 | 106 | %%%%%%% 107 | 108 | 109 | %%%%% update_state 110 | for i=1:Number_NodeTypes 111 | ThisTypeName=NodeTypeNamesArray{i}; 112 | dx = dX (Graph.Nodes.( ThisTypeName ).updateIndex); 113 | dimension_thisType= Graph.Nodes.( ThisTypeName ).Dimension; 114 | 115 | all_ids_thisType= fields( Graph.Nodes.( ThisTypeName ).Values ); 116 | num_ids_thisType= size ( all_ids_thisType, 1 ); 117 | 118 | for k=1:num_ids_thisType 119 | thisNode_id = all_ids_thisType{k}; 120 | increment = dx ( dimension_thisType*(k-1)+1 : dimension_thisType*k ); 121 | Graph.Nodes.( ThisTypeName ).Values.( thisNode_id ) = update_state( ThisTypeName, Graph.Nodes.( ThisTypeName ).Values.( thisNode_id ) , increment); 122 | end 123 | end 124 | 125 | F_new = Cal_ObjValue( Graph ); 126 | 127 | p=(F_0 - F_new)/(L_0 - L_new); 128 | if p>0 129 | stop = (rasidual<= eplison_3) || ( g_inf <= eplison_1 ); 130 | iter_optimization=iter_optimization+1; 131 | if F_0-F_new < eplison_3 132 | stop = 1; 133 | end 134 | else 135 | Graph.Nodes=previousNodes; 136 | end 137 | delta = update_radius(p, h_dl, delta ); 138 | 139 | end 140 | 141 | Graph.iter_actual = iter_optimization; 142 | 143 | end 144 | -------------------------------------------------------------------------------- /g2o_files/PerformGO_LM.m: -------------------------------------------------------------------------------- 1 | function [ Graph ] = PerformGO_LM( Graph ) 2 | %%%%%%%%%%%%%% generate the sparse Jaocbian matrix and the error vecotr 3 | Total_ErrorVector=zeros( Graph.TotalDimensionOfEdges, 1 ); %%%%% Initialize the Error Vector 4 | Number_NodeTypes=size( fields(Graph.Nodes) ,1 ); 5 | Total_Jacobian=struct; 6 | NodeTypeNamesArray = fields(Graph.Nodes); 7 | 8 | num_update=0; 9 | for i=1:Number_NodeTypes 10 | Total_Jacobian.( NodeTypeNamesArray{i} )=[]; 11 | dimension= Graph.Nodes.( NodeTypeNamesArray{i} ).Dimension; 12 | num_node_thisKind = size( fields (Graph.Nodes.( NodeTypeNamesArray{i} ).Values) , 1) ; 13 | 14 | if isfield(Graph.Fixed, NodeTypeNamesArray{i}) 15 | num_node_thisKind = num_node_thisKind - Graph.Fixed.(NodeTypeNamesArray{i}); 16 | end 17 | 18 | 19 | Graph.Nodes.( NodeTypeNamesArray{i} ).updateIndex = num_update+ [1:1: dimension* num_node_thisKind ]'; 20 | num_update=num_update+ dimension* num_node_thisKind; 21 | end 22 | 23 | NumberofEdges=size(Graph.Edges, 2); 24 | NeedOptimization=true; 25 | iter_optimization=0; 26 | 27 | v=2; 28 | 29 | Graph.Edge_Error = zeros( NumberofEdges,1 ); 30 | 31 | while ( NeedOptimization && iter_optimization < Graph.Parameters.iter ) 32 | NeedOptimization_possible = false; 33 | 34 | for iter_edge=1:NumberofEdges 35 | NeedOptimization=true; 36 | edge =Graph.Edges{iter_edge}; 37 | edgeType=edge.EdgeType; 38 | 39 | num_nodes_ThisEdge= size( fieldnames(edge.withNodes),1); 40 | Nodes_array=cell(num_nodes_ThisEdge, 1 ); 41 | nodes_id = fields(edge.withNodes); 42 | for i = 1: num_nodes_ThisEdge 43 | node_id = nodes_id{i}; 44 | node_type= edge.withNodes.(node_id).NodeTypeName; 45 | Nodes_array{i}=Graph.Nodes.(node_type).Values.(node_id); 46 | end 47 | FuncFactor=str2func(edgeType); 48 | [ ErrorVector, Jacobian_Node ]=FuncFactor( Nodes_array, edge.Measurement.value ); 49 | 50 | Total_ErrorVector( edge.ErrorVectorIndex )= edge.Measurement.inf_sqrt* ErrorVector; 51 | Graph.Edge_Error( iter_edge ) = (edge.Measurement.inf_sqrt* ErrorVector)'*(edge.Measurement.inf_sqrt* ErrorVector); 52 | 53 | for i = 1: num_nodes_ThisEdge 54 | node_id = nodes_id{i}; 55 | node_type= edge.withNodes.(node_id).NodeTypeName; 56 | 57 | if ~isfield( Graph.Fixed.IDname, node_id) 58 | Jacobian_Node_i_infsqrt=edge.Measurement.inf_sqrt*Jacobian_Node{i}; 59 | Jacobianindex_thisEdge_thisNode=edge.withNodes.(node_id).JacobianIndex; 60 | Graph.Nodes.(node_type).Jacobian.ValueVector( Jacobianindex_thisEdge_thisNode )=Jacobian_Node_i_infsqrt(:); 61 | end 62 | 63 | 64 | end 65 | 66 | end 67 | 68 | F_0 = Total_ErrorVector'*Total_ErrorVector; 69 | L_0 = F_0; 70 | sprintf('cost = %f\n',Total_ErrorVector'*Total_ErrorVector ) 71 | 72 | 73 | 74 | Whole_Jacobian=[]; 75 | for i=1:Number_NodeTypes 76 | ThisTypeName=NodeTypeNamesArray{i}; 77 | all_ids_thisType= fields( Graph.Nodes.( ThisTypeName ).Values ); 78 | num_ids_thisType= size ( all_ids_thisType, 1 ); 79 | 80 | num_ids_thisType = num_ids_thisType - Graph.Fixed.(ThisTypeName); % new 81 | 82 | 83 | num_cols=num_ids_thisType*Graph.Nodes.(ThisTypeName).Dimension; 84 | num_rows=Graph.TotalDimensionOfEdges; 85 | 86 | Total_Jacobian.( NodeTypeNamesArray{i} )=sparse( Graph.Nodes.(ThisTypeName).Jacobian.RowVector, Graph.Nodes.(ThisTypeName).Jacobian.ColVector, Graph.Nodes.(ThisTypeName).Jacobian.ValueVector, num_rows, num_cols ); 87 | Whole_Jacobian=[Whole_Jacobian Total_Jacobian.( NodeTypeNamesArray{i} )]; 88 | end 89 | 90 | G = Whole_Jacobian'*Total_ErrorVector; 91 | 92 | if iter_optimization == 0 93 | AAA= Whole_Jacobian'*Whole_Jacobian; 94 | u = max(diag( AAA ))* Graph.Parameters.tau ; 95 | end 96 | 97 | 98 | dX= (- u*speye( num_update )-Whole_Jacobian'*Whole_Jacobian)\(Whole_Jacobian'*Total_ErrorVector); % this is the incremental vector 99 | 100 | L_new = (Total_ErrorVector+Whole_Jacobian*dX)'*(Total_ErrorVector+Whole_Jacobian*dX); 101 | 102 | previousNodes = Graph.Nodes; 103 | 104 | 105 | %%%%% update_state 106 | for i=1:Number_NodeTypes 107 | ThisTypeName=NodeTypeNamesArray{i}; 108 | dx = dX (Graph.Nodes.( ThisTypeName ).updateIndex); 109 | dimension_thisType= Graph.Nodes.( ThisTypeName ).Dimension; 110 | 111 | all_ids_thisType= fields( Graph.Nodes.( ThisTypeName ).Values ); 112 | num_ids_thisType= size ( all_ids_thisType, 1 ); 113 | 114 | PreFixed_num = 0; 115 | for k=1:num_ids_thisType 116 | thisNode_id = all_ids_thisType{k}; 117 | 118 | if ~isfield(Graph.Fixed.IDname, thisNode_id) 119 | %%increment = dx ( dimension_thisType*(k-1)+1 : dimension_thisType*k ); %%% 120 | increment = dx ( dimension_thisType*(k-1-PreFixed_num)+1 : dimension_thisType*(k-PreFixed_num) ); 121 | 122 | Z= abs( increment )-Graph.Parameters.LinearizedError.(ThisTypeName); 123 | if any(Z>0) 124 | NeedOptimization_possible=true; 125 | end 126 | 127 | Graph.Nodes.( ThisTypeName ).Values.( thisNode_id ) = update_state( ThisTypeName, Graph.Nodes.( ThisTypeName ).Values.( thisNode_id ) , increment); 128 | else 129 | PreFixed_num = PreFixed_num+1; 130 | end 131 | 132 | 133 | 134 | end 135 | end 136 | 137 | F_new = Cal_ObjValue( Graph ); 138 | 139 | 140 | %p2 = dX'*(u*dX+max(abs(G))); 141 | %p=(F_0 - F_new)/p2; 142 | 143 | p=(F_0 - F_new)/(L_0 - L_new); 144 | if p>0 145 | u= u* max(1/3, 1-(2*p-1)^3); 146 | v=2; 147 | if abs(F_0-F_new) < Graph.Parameters.eplison*F_0 148 | NeedOptimization = false; Graph.stop_reason = 'obj is small'; 149 | end 150 | % if NeedOptimization_possible 151 | % else NeedOptimization = false; Graph.stop_reason = 'dX is small'; 152 | % end 153 | iter_optimization=iter_optimization+1; 154 | else 155 | Graph.Nodes=previousNodes; 156 | u = u*v; 157 | v=2*v; 158 | NeedOptimization = true; 159 | end 160 | end 161 | 162 | Graph.iter_actual = iter_optimization; 163 | 164 | end 165 | -------------------------------------------------------------------------------- /g2o_files/calculate_inverse.m: -------------------------------------------------------------------------------- 1 | function invHss = calculate_inverse( Hss, NodeTable) 2 | invHss = Hss; 3 | types = fields(NodeTable.schur); 4 | 5 | num_types = size(types, 1); 6 | 7 | for i = 1:num_types 8 | type_i = types{i}; 9 | dimension = NodeTable.schur.(type_i).dimension; 10 | upindex = NodeTable.schur.(type_i).updateIndex; 11 | this_block = upindex(1):dimension:upindex(end); 12 | 13 | num_i = (upindex(end)+1 - upindex(1) )/dimension; 14 | 15 | for j = 1:num_i 16 | index_ij = this_block(j):(this_block(j)+dimension-1); 17 | A = Hss( index_ij ,index_ij ); 18 | invHss( index_ij, index_ij ) = inv(A); 19 | end 20 | 21 | end 22 | 23 | 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /g2o_files/calculate_inverse_new.m: -------------------------------------------------------------------------------- 1 | function invHss = calculate_inverse_new( Hss, NodeTable) 2 | %load matlab.mat; 3 | 4 | types = fields(NodeTable.schur); 5 | NodeTable = Fn_processNodeTable( NodeTable ); 6 | 7 | [ index_r, index_c, values ] = find( Hss ); 8 | 9 | num_values = length( index_r ); 10 | 11 | 12 | index_type = 1; 13 | type_i = types{index_type}; 14 | dimension = NodeTable.schur.(type_i).dimension; 15 | 16 | 17 | i=1; 18 | 19 | while i<=num_values 20 | 21 | while i > max( NodeTable.schur.(types{index_type}).num ) 22 | index_type = index_type+1; 23 | dimension = NodeTable.schur.(type_i).dimension; 24 | end 25 | 26 | aa = reshape(values(i:i+dimension^2-1),dimension,dimension); 27 | aa = inv(aa); 28 | 29 | values( i:i+dimension^2-1 ) = reshape( aa, dimension^2 , 1 ); 30 | i=i+dimension^2; 31 | end 32 | 33 | invHss = sparse( index_r, index_c, values ); 34 | 35 | inv(Hss); 36 | 37 | 38 | end 39 | 40 | 41 | -------------------------------------------------------------------------------- /g2o_files/classifyNodeType.m: -------------------------------------------------------------------------------- 1 | function [ NodeTable ] = classifyNodeType( NodeTypeNamesArray, schur ) 2 | 3 | Number_NodeTypes=size( NodeTypeNamesArray ,1 ); 4 | 5 | for i = 1:Number_NodeTypes 6 | NodeType_i = NodeTypeNamesArray{i}; 7 | dimension_i = GetNodeTypeDimension(NodeType_i); 8 | 9 | if isfield( schur, NodeType_i) 10 | %NodeTable.schur.(NodeType_i)=[]; 11 | NodeTable.schur.(NodeType_i).dimension=dimension_i; 12 | else 13 | %NodeTable.normal.(NodeType_i)=[]; 14 | NodeTable.normal.(NodeType_i).dimension=dimension_i; 15 | end 16 | end 17 | 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /g2o_files/compute_DogLeg.m: -------------------------------------------------------------------------------- 1 | function h_dl=compute_DogLeg( h_gn, h_sd , delta ) 2 | 3 | norm_hgn = norm(h_gn); 4 | norm_hsd = norm(h_sd); 5 | if norm_hgn <= delta 6 | h_dl = h_gn; 7 | else if norm_hsd>delta 8 | h_dl = ( delta/norm(h_sd) ) * h_sd; 9 | else 10 | beta = compute_beta( h_gn , h_sd , delta ); 11 | h_dl = h_sd + beta*(h_gn - h_sd); 12 | end 13 | 14 | 15 | 16 | end 17 | 18 | end 19 | 20 | function beta =compute_beta( h_gn , h_sd , delta ) 21 | 22 | a = h_sd; 23 | b = h_gn; 24 | c = a'*(b-a); 25 | 26 | A2 = a'*a; 27 | BA2 = (b-a)'*(b-a); 28 | 29 | 30 | if c<= 0 31 | beta = (-c+sqrt( c^2 + BA2*( delta^2 - A2 ) ))/BA2; 32 | 33 | else 34 | beta = (delta^2 - A2)/( c+ sqrt( c^2 + BA2*( delta^2 - A2 ) ) ); 35 | 36 | end 37 | 38 | 39 | end -------------------------------------------------------------------------------- /g2o_files/update_radius.m: -------------------------------------------------------------------------------- 1 | function delta = update_radius(p, h_dl, delta ) 2 | 3 | if p>0.75 4 | delta = max(delta, 3*norm(h_dl)); 5 | else if p<0.25 6 | delta = delta/2; 7 | end 8 | 9 | end --------------------------------------------------------------------------------