12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

"fixed" Unstable MATLAB examples - untested

release/4.3a0
Frank Dellaert 2022-01-02 14:46:39 -05:00
parent 70092ca18a
commit 8dbe5ee179
8 changed files with 16 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
matlab/+gtsam/Contents.m
View File

@ -49,9 +49,6 @@
% Ordering - class Ordering, see Doxygen page for details % Ordering - class Ordering, see Doxygen page for details
% Value - class Value, see Doxygen page for details % Value - class Value, see Doxygen page for details
% Values - class Values, see Doxygen page for details % Values - class Values, see Doxygen page for details
% LieScalar - class LieScalar, see Doxygen page for details
% LieVector - class LieVector, see Doxygen page for details
% LieMatrix - class LieMatrix, see Doxygen page for details
% NonlinearFactor - class NonlinearFactor, see Doxygen page for details % NonlinearFactor - class NonlinearFactor, see Doxygen page for details
% NonlinearFactorGraph - class NonlinearFactorGraph, see Doxygen page for details % NonlinearFactorGraph - class NonlinearFactorGraph, see Doxygen page for details
% %
@ -101,9 +98,6 @@
% BearingFactor2D - class BearingFactor2D, see Doxygen page for details % BearingFactor2D - class BearingFactor2D, see Doxygen page for details
% BearingFactor3D - class BearingFactor3D, see Doxygen page for details % BearingFactor3D - class BearingFactor3D, see Doxygen page for details
% BearingRangeFactor2D - class BearingRangeFactor2D, see Doxygen page for details % BearingRangeFactor2D - class BearingRangeFactor2D, see Doxygen page for details
% BetweenFactorLieMatrix - class BetweenFactorLieMatrix, see Doxygen page for details
% BetweenFactorLieScalar - class BetweenFactorLieScalar, see Doxygen page for details
% BetweenFactorLieVector - class BetweenFactorLieVector, see Doxygen page for details
% BetweenFactorPoint2 - class BetweenFactorPoint2, see Doxygen page for details % BetweenFactorPoint2 - class BetweenFactorPoint2, see Doxygen page for details
% BetweenFactorPoint3 - class BetweenFactorPoint3, see Doxygen page for details % BetweenFactorPoint3 - class BetweenFactorPoint3, see Doxygen page for details
% BetweenFactorPose2 - class BetweenFactorPose2, see Doxygen page for details % BetweenFactorPose2 - class BetweenFactorPose2, see Doxygen page for details
@ -116,9 +110,6 @@
% GenericStereoFactor3D - class GenericStereoFactor3D, see Doxygen page for details % GenericStereoFactor3D - class GenericStereoFactor3D, see Doxygen page for details
% NonlinearEqualityCal3_S2 - class NonlinearEqualityCal3_S2, see Doxygen page for details % NonlinearEqualityCal3_S2 - class NonlinearEqualityCal3_S2, see Doxygen page for details
% NonlinearEqualityCalibratedCamera - class NonlinearEqualityCalibratedCamera, see Doxygen page for details % NonlinearEqualityCalibratedCamera - class NonlinearEqualityCalibratedCamera, see Doxygen page for details
% NonlinearEqualityLieMatrix - class NonlinearEqualityLieMatrix, see Doxygen page for details
% NonlinearEqualityLieScalar - class NonlinearEqualityLieScalar, see Doxygen page for details
% NonlinearEqualityLieVector - class NonlinearEqualityLieVector, see Doxygen page for details
% NonlinearEqualityPoint2 - class NonlinearEqualityPoint2, see Doxygen page for details % NonlinearEqualityPoint2 - class NonlinearEqualityPoint2, see Doxygen page for details
% NonlinearEqualityPoint3 - class NonlinearEqualityPoint3, see Doxygen page for details % NonlinearEqualityPoint3 - class NonlinearEqualityPoint3, see Doxygen page for details
% NonlinearEqualityPose2 - class NonlinearEqualityPose2, see Doxygen page for details % NonlinearEqualityPose2 - class NonlinearEqualityPose2, see Doxygen page for details
@ -129,9 +120,6 @@
% NonlinearEqualityStereoPoint2 - class NonlinearEqualityStereoPoint2, see Doxygen page for details % NonlinearEqualityStereoPoint2 - class NonlinearEqualityStereoPoint2, see Doxygen page for details
% PriorFactorCal3_S2 - class PriorFactorCal3_S2, see Doxygen page for details % PriorFactorCal3_S2 - class PriorFactorCal3_S2, see Doxygen page for details
% PriorFactorCalibratedCamera - class PriorFactorCalibratedCamera, see Doxygen page for details % PriorFactorCalibratedCamera - class PriorFactorCalibratedCamera, see Doxygen page for details
% PriorFactorLieMatrix - class PriorFactorLieMatrix, see Doxygen page for details
% PriorFactorLieScalar - class PriorFactorLieScalar, see Doxygen page for details
% PriorFactorLieVector - class PriorFactorLieVector, see Doxygen page for details
% PriorFactorPoint2 - class PriorFactorPoint2, see Doxygen page for details % PriorFactorPoint2 - class PriorFactorPoint2, see Doxygen page for details
% PriorFactorPoint3 - class PriorFactorPoint3, see Doxygen page for details % PriorFactorPoint3 - class PriorFactorPoint3, see Doxygen page for details
% PriorFactorPose2 - class PriorFactorPose2, see Doxygen page for details % PriorFactorPose2 - class PriorFactorPose2, see Doxygen page for details

8
matlab/unstable_examples/+imuSimulator/IMUComparison.m
View File

@ -51,13 +51,13 @@ isam = gtsam.ISAM2(isamParams);
initialValues = Values; initialValues = Values;
initialValues.insert(symbol('x',0), currentPoseGlobal); initialValues.insert(symbol('x',0), currentPoseGlobal);
initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); initialValues.insert(symbol('v',0), currentVelocityGlobal);
initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
initialFactors = NonlinearFactorGraph; initialFactors = NonlinearFactorGraph;
initialFactors.add(PriorFactorPose3(symbol('x',0), ... initialFactors.add(PriorFactorPose3(symbol('x',0), ...
currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0))); currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0)));
initialFactors.add(PriorFactorLieVector(symbol('v',0), ... initialFactors.add(PriorFactorVector(symbol('v',0), ...
LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0))); currentVelocityGlobal, noiseModel.Isotropic.Sigma(3, 1.0)));
initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0))); imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0)));
@ -96,7 +96,7 @@ for t = times
initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
else else
initialPose = Pose3; initialPose = Pose3;
initialVel = LieVector(velocity); initialVel = velocity;
end end
initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel);

8
matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m
View File

@ -43,15 +43,15 @@ sigma_init_b = 1.0;
initialValues = Values; initialValues = Values;
initialValues.insert(symbol('x',0), currentPoseGlobal); initialValues.insert(symbol('x',0), currentPoseGlobal);
initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); initialValues.insert(symbol('v',0), currentVelocityGlobal);
initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
initialFactors = NonlinearFactorGraph; initialFactors = NonlinearFactorGraph;
% Prior on initial pose % Prior on initial pose
initialFactors.add(PriorFactorPose3(symbol('x',0), ... initialFactors.add(PriorFactorPose3(symbol('x',0), ...
currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x))); currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x)));
% Prior on initial velocity % Prior on initial velocity
initialFactors.add(PriorFactorLieVector(symbol('v',0), ... initialFactors.add(PriorFactorVector(symbol('v',0), ...
LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v))); currentVelocityGlobal, noiseModel.Isotropic.Sigma(3, sigma_init_v)));
% Prior on initial bias % Prior on initial bias
initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b))); imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b)));
@ -91,7 +91,7 @@ for t = times
initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
else else
initialPose = Pose3; initialPose = Pose3;
initialVel = LieVector(velocity); initialVel = velocity;
end end
initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel);

6
matlab/unstable_examples/+imuSimulator/coriolisExample.m
View File

@ -175,9 +175,9 @@ for i = 1:length(times)
% known initial conditions % known initial conditions
currentPoseEstimate = currentPoseFixedGT; currentPoseEstimate = currentPoseFixedGT;
if navFrameRotating == 1 if navFrameRotating == 1
currentVelocityEstimate = LieVector(currentVelocityRotatingGT); currentVelocityEstimate = currentVelocityRotatingGT;
else else
currentVelocityEstimate = LieVector(currentVelocityFixedGT); currentVelocityEstimate = currentVelocityFixedGT;
end end
% Set Priors % Set Priors
@ -186,7 +186,7 @@ for i = 1:length(times)
newValues.insert(currentBiasKey, zeroBias); newValues.insert(currentBiasKey, zeroBias);
% Initial values, same for IMU types 1 and 2 % Initial values, same for IMU types 1 and 2
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x)); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v)); newFactors.add(PriorFactorVector(currentVelKey, currentVelocityEstimate, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
% Store data % Store data

2
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -27,7 +27,7 @@ for i=0:length(measurements)
if options.includeIMUFactors == 1 if options.includeIMUFactors == 1
currentVelKey = symbol('v', 0); currentVelKey = symbol('v', 0);
currentVel = values.atPoint3(currentVelKey); currentVel = values.atPoint3(currentVelKey);
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); graph.add(PriorFactorVector(currentVelKey, currentVel, noiseModels.noiseVel));
currentBiasKey = symbol('b', 0); currentBiasKey = symbol('b', 0);
currentBias = values.atPoint3(currentBiasKey); currentBias = values.atPoint3(currentBiasKey);

2
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -82,7 +82,7 @@ if options.useRealData == 1
end end
% Add Values: velocity and bias % Add Values: velocity and bias
values.insert(currentVelKey, LieVector(currentVel)); values.insert(currentVelKey, currentVel);
values.insert(currentBiasKey, metadata.imu.zeroBias); values.insert(currentBiasKey, metadata.imu.zeroBias);
end end

2
matlab/unstable_examples/FlightCameraTransformIMU.m
View File

@ -167,7 +167,7 @@ for i=1:size(trajectory)-1
%% priors on first two poses %% priors on first two poses
if i < 3 if i < 3
% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); % fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
end end

4
matlab/unstable_examples/IMUKittiExampleVO.m
View File

@ -46,7 +46,7 @@ clear logposes relposes
%% Get initial conditions for the estimated trajectory %% Get initial conditions for the estimated trajectory
currentPoseGlobal = Pose3; currentPoseGlobal = Pose3;
currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_x = noiseModel.Isotropic.Sigmas([ 1.0; 1.0; 0.01; 0.01; 0.01; 0.01 ]); sigma_init_x = noiseModel.Isotropic.Sigmas([ 1.0; 1.0; 0.01; 0.01; 0.01; 0.01 ]);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
@ -88,7 +88,7 @@ for measurementIndex = 1:length(timestamps)
newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentVelKey, currentVelocityGlobal);
newValues.insert(currentBiasKey, currentBias); newValues.insert(currentBiasKey, currentBias);
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
else else
t_previous = timestamps(measurementIndex-1, 1); t_previous = timestamps(measurementIndex-1, 1);