adapt examples to new direct member access
parent
11b2135eb3
commit
65d72ab7a1
|
@ -7,7 +7,7 @@ import gtsam.*
|
||||||
%% Initialize iSAM
|
%% Initialize iSAM
|
||||||
params = gtsam.ISAM2Params;
|
params = gtsam.ISAM2Params;
|
||||||
if options.alwaysRelinearize
|
if options.alwaysRelinearize
|
||||||
params.setRelinearizeSkip(1);
|
params.relinearizeSkip = 1;
|
||||||
end
|
end
|
||||||
isam = ISAM2(params);
|
isam = ISAM2(params);
|
||||||
|
|
||||||
|
|
|
@ -52,7 +52,7 @@ IMU_params.setOmegaCoriolis(w_coriolis);
|
||||||
%% Solver object
|
%% Solver object
|
||||||
isamParams = ISAM2Params;
|
isamParams = ISAM2Params;
|
||||||
isamParams.setFactorization('CHOLESKY');
|
isamParams.setFactorization('CHOLESKY');
|
||||||
isamParams.setRelinearizeSkip(10);
|
isamParams.relinearizeSkip = 10;
|
||||||
isam = gtsam.ISAM2(isamParams);
|
isam = gtsam.ISAM2(isamParams);
|
||||||
newFactors = NonlinearFactorGraph;
|
newFactors = NonlinearFactorGraph;
|
||||||
newValues = Values;
|
newValues = Values;
|
||||||
|
|
|
@ -46,7 +46,7 @@ posesIMUbody(1).R = poses(1).R;
|
||||||
|
|
||||||
%% Solver object
|
%% Solver object
|
||||||
isamParams = ISAM2Params;
|
isamParams = ISAM2Params;
|
||||||
isamParams.setRelinearizeSkip(1);
|
isamParams.relinearizeSkip = 1;
|
||||||
isam = gtsam.ISAM2(isamParams);
|
isam = gtsam.ISAM2(isamParams);
|
||||||
|
|
||||||
initialValues = Values;
|
initialValues = Values;
|
||||||
|
|
|
@ -34,7 +34,7 @@ poses(1).R = currentPoseGlobal.rotation.matrix;
|
||||||
|
|
||||||
%% Solver object
|
%% Solver object
|
||||||
isamParams = ISAM2Params;
|
isamParams = ISAM2Params;
|
||||||
isamParams.setRelinearizeSkip(1);
|
isamParams.relinearizeSkip = 1;
|
||||||
isam = gtsam.ISAM2(isamParams);
|
isam = gtsam.ISAM2(isamParams);
|
||||||
|
|
||||||
sigma_init_x = 1.0;
|
sigma_init_x = 1.0;
|
||||||
|
|
|
@ -119,7 +119,7 @@ h = figure;
|
||||||
% Solver object
|
% Solver object
|
||||||
isamParams = ISAM2Params;
|
isamParams = ISAM2Params;
|
||||||
isamParams.setFactorization('CHOLESKY');
|
isamParams.setFactorization('CHOLESKY');
|
||||||
isamParams.setRelinearizeSkip(10);
|
isamParams.relinearizeSkip = 10;
|
||||||
isam = gtsam.ISAM2(isamParams);
|
isam = gtsam.ISAM2(isamParams);
|
||||||
newFactors = NonlinearFactorGraph;
|
newFactors = NonlinearFactorGraph;
|
||||||
newValues = Values;
|
newValues = Values;
|
||||||
|
|
|
@ -82,7 +82,7 @@ w_coriolis = [0;0;0];
|
||||||
%% Solver object
|
%% Solver object
|
||||||
isamParams = ISAM2Params;
|
isamParams = ISAM2Params;
|
||||||
isamParams.setFactorization('QR');
|
isamParams.setFactorization('QR');
|
||||||
isamParams.setRelinearizeSkip(1);
|
isamParams.relinearizeSkip = 1;
|
||||||
isam = gtsam.ISAM2(isamParams);
|
isam = gtsam.ISAM2(isamParams);
|
||||||
newFactors = NonlinearFactorGraph;
|
newFactors = NonlinearFactorGraph;
|
||||||
newValues = Values;
|
newValues = Values;
|
||||||
|
|
|
@ -58,7 +58,7 @@ w_coriolis = [0;0;0];
|
||||||
%% Solver object
|
%% Solver object
|
||||||
isamParams = ISAM2Params;
|
isamParams = ISAM2Params;
|
||||||
isamParams.setFactorization('CHOLESKY');
|
isamParams.setFactorization('CHOLESKY');
|
||||||
isamParams.setRelinearizeSkip(10);
|
isamParams.relinearizeSkip = 10;
|
||||||
isam = gtsam.ISAM2(isamParams);
|
isam = gtsam.ISAM2(isamParams);
|
||||||
newFactors = NonlinearFactorGraph;
|
newFactors = NonlinearFactorGraph;
|
||||||
newValues = Values;
|
newValues = Values;
|
||||||
|
|
|
@ -203,7 +203,7 @@ def optimize(gps_measurements: List[GpsMeasurement],
|
||||||
# Set ISAM2 parameters and create ISAM2 solver object
|
# Set ISAM2 parameters and create ISAM2 solver object
|
||||||
isam_params = gtsam.ISAM2Params()
|
isam_params = gtsam.ISAM2Params()
|
||||||
isam_params.setFactorization("CHOLESKY")
|
isam_params.setFactorization("CHOLESKY")
|
||||||
isam_params.setRelinearizeSkip(10)
|
isam_params.relinearizeSkip = 10
|
||||||
|
|
||||||
isam = gtsam.ISAM2(isam_params)
|
isam = gtsam.ISAM2(isam_params)
|
||||||
|
|
||||||
|
|
|
@ -111,7 +111,7 @@ def Pose2SLAM_ISAM2_example():
|
||||||
# update calls are required to perform the relinearization.
|
# update calls are required to perform the relinearization.
|
||||||
parameters = gtsam.ISAM2Params()
|
parameters = gtsam.ISAM2Params()
|
||||||
parameters.setRelinearizeThreshold(0.1)
|
parameters.setRelinearizeThreshold(0.1)
|
||||||
parameters.setRelinearizeSkip(1)
|
parameters.relinearizeSkip = 1
|
||||||
isam = gtsam.ISAM2(parameters)
|
isam = gtsam.ISAM2(parameters)
|
||||||
|
|
||||||
# Create the ground truth odometry measurements of the robot during the trajectory.
|
# Create the ground truth odometry measurements of the robot during the trajectory.
|
||||||
|
|
|
@ -140,7 +140,7 @@ def Pose3_ISAM2_example():
|
||||||
# update calls are required to perform the relinearization.
|
# update calls are required to perform the relinearization.
|
||||||
parameters = gtsam.ISAM2Params()
|
parameters = gtsam.ISAM2Params()
|
||||||
parameters.setRelinearizeThreshold(0.1)
|
parameters.setRelinearizeThreshold(0.1)
|
||||||
parameters.setRelinearizeSkip(1)
|
parameters.relinearizeSkip = 1
|
||||||
isam = gtsam.ISAM2(parameters)
|
isam = gtsam.ISAM2(parameters)
|
||||||
|
|
||||||
# Create the ground truth poses of the robot trajectory.
|
# Create the ground truth poses of the robot trajectory.
|
||||||
|
|
|
@ -81,7 +81,7 @@ def visual_ISAM2_example():
|
||||||
# will approach the batch result.
|
# will approach the batch result.
|
||||||
parameters = gtsam.ISAM2Params()
|
parameters = gtsam.ISAM2Params()
|
||||||
parameters.setRelinearizeThreshold(0.01)
|
parameters.setRelinearizeThreshold(0.01)
|
||||||
parameters.setRelinearizeSkip(1)
|
parameters.relinearizeSkip = 1
|
||||||
isam = gtsam.ISAM2(parameters)
|
isam = gtsam.ISAM2(parameters)
|
||||||
|
|
||||||
# Create a Factor Graph and Values to hold the new data
|
# Create a Factor Graph and Values to hold the new data
|
||||||
|
|
|
@ -17,7 +17,7 @@ def initialize(data, truth, options):
|
||||||
# Initialize iSAM
|
# Initialize iSAM
|
||||||
params = gtsam.ISAM2Params()
|
params = gtsam.ISAM2Params()
|
||||||
if options.alwaysRelinearize:
|
if options.alwaysRelinearize:
|
||||||
params.setRelinearizeSkip(1)
|
params.relinearizeSkip = 1
|
||||||
isam = gtsam.ISAM2(params=params)
|
isam = gtsam.ISAM2(params=params)
|
||||||
|
|
||||||
# Add constraints/priors
|
# Add constraints/priors
|
||||||
|
|
Loading…
Reference in New Issue