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

Ported MATLAB Examples

release/4.3a0
Fan Jiang 2020-08-17 14:37:12 -04:00
parent 37f42b82fa
commit 840831b62b
45 changed files with 213 additions and 178 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
matlab/+gtsam/Point2.m Normal file
View File

@ -0,0 +1,12 @@
function pt = Point2(varargin)
% Point2 shim
if nargin == 2 && isa(varargin{1}, 'double')
pt = [varargin{1} varargin{2}]';
elseif nargin == 1
pt = varargin{1};
elseif nargin == 0
pt = [0 0]';
else
error('Arguments do not match any overload of Point2 shim');
end
end

12
matlab/+gtsam/Point3.m Normal file
View File

@ -0,0 +1,12 @@
function pt = Point3(varargin)
% Point3 shim
if nargin == 3 && isa(varargin{1}, 'double')
pt = [varargin{1} varargin{2} varargin{3}]';
elseif nargin == 1
pt = varargin{1};
elseif nargin == 0
pt = [0 0 0]';
else
error('Arguments do not match any overload of Point3 shim');
end
end

6
matlab/+gtsam/cylinderSampleProjection.m
View File

@ -50,9 +50,9 @@ for i = 1:cylinderNum
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
rayCameraToPoint = pose.translation().between(sampledPoint3);
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid);
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3);
% Condition 1: all points in front of the cylinders'
% surfaces are visible

20
matlab/+gtsam/cylinderSampleProjectionStereo.m
View File

@ -25,20 +25,20 @@ for i = 1:cylinderNum
% For Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transformTo(sampledPoint3);
if sampledPoint3local.z < 0
if sampledPoint3local(3) < 0
continue;
end
% measurements
Z.du = K.fx() * K.baseline() / sampledPoint3local.z;
Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
Z.du = K.fx() * K.baseline() / sampledPoint3local(3);
Z.uL = K.fx() * sampledPoint3local(1) / sampledPoint3local(3) + K.px();
Z.uR = Z.uL + Z.du;
Z.v = K.fy() / sampledPoint3local.z + K.py();
Z.v = K.fy() / sampledPoint3local(3) + K.py();
% ignore points not visible in the scene
if Z.uL < 0 || Z.uL >= imageSize.x || ...
Z.uR < 0 || Z.uR >= imageSize.x || ...
Z.v < 0 || Z.v >= imageSize.y
if Z.uL < 0 || Z.uL >= imageSize(1) || ...
Z.uR < 0 || Z.uR >= imageSize(1) || ...
Z.v < 0 || Z.v >= imageSize(2)
continue;
end
@ -54,9 +54,9 @@ for i = 1:cylinderNum
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
rayCameraToPoint = sampledPoint3 - pose.translation();
rayCameraToCylinder = cylinders{k}.centroid - pose.translation();
rayCylinderToPoint = sampledPoint3 - cylinders{k}.centroid;
% Condition 1: all points in front of the cylinders'
% surfaces are visible

6
matlab/+gtsam/cylinderSampling.m
View File

@ -12,8 +12,8 @@ function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
% sample the points
for i = 1:pointsNum
theta = 2 * pi * rand;
x = radius * cos(theta) + baseCentroid.x;
y = radius * sin(theta) + baseCentroid.y;
x = radius * cos(theta) + baseCentroid(1);
y = radius * sin(theta) + baseCentroid(2);
z = height * rand;
points3{i,1} = Point3([x,y,z]');
end
@ -22,5 +22,5 @@ function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
cylinder.radius = radius;
cylinder.height = height;
cylinder.Points = points3;
cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
cylinder.centroid = Point3(baseCentroid(1), baseCentroid(2), height/2);
end

2
matlab/+gtsam/plotCamera.m
View File

@ -1,7 +1,7 @@
function plotCamera(pose, axisLength)
hold on
C = pose.translation().vector();
C = pose.translation();
R = pose.rotation().matrix();
xAxis = C+R(:,1)*axisLength;

16
matlab/+gtsam/plotFlyingResults.m
View File

@ -13,7 +13,7 @@ set(gcf, 'Position', [80,1,1800,1000]);
%% plot all the cylinders and sampled points
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
axis([0, options.fieldSize(1), 0, options.fieldSize(2), 0, options.height + 30]);
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Height (m)');
@ -50,8 +50,8 @@ for i = 1:cylinderNum
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
X = X + cylinders{i}.centroid.x;
Y = Y + cylinders{i}.centroid.y;
X = X + cylinders{i}.centroid(1);
Y = Y + cylinders{i}.centroid(2);
Z = Z * cylinders{i}.height;
h_cylinder{i} = surf(X,Y,Z);
@ -76,7 +76,7 @@ for i = 1:posesSize
%plotCamera(poses{i}, 3);
gRp = poses{i}.rotation().matrix(); % rotation from pose to global
C = poses{i}.translation().vector();
C = poses{i}.translation();
axisLength = 3;
xAxis = C+gRp(:,1)*axisLength;
@ -111,14 +111,14 @@ for i = 1:posesSize
for j = 1:pointSize
if ~isempty(pts3d{j}.cov{i})
hold on
h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
h_point{j} = plot3(pts3d{j}.data(1), pts3d{j}.data(2), pts3d{j}.data(3));
h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data(1); pts3d{j}.data(2); pts3d{j}.data(3)], ...
pts3d{j}.cov{i}, options.plot.covarianceScale);
end
end
axis equal
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
axis([0, options.fieldSize(1), 0, options.fieldSize(2), 0, options.height + 30]);
drawnow
@ -158,7 +158,7 @@ for i = 1 : posesSize
hold on
campos([poses{i}.x, poses{i}.y, poses{i}.z]);
camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
camtarget([options.fieldSize(1)/2, options.fieldSize(2)/2, 0]);
camlight(hlight, 'headlight');
if options.writeVideo

6
matlab/+gtsam/plotPoint2.m
View File

@ -1,10 +1,10 @@
function plotPoint2(p,color,P)
% plotPoint2 shows a Point2, possibly with covariance matrix
if size(color,2)==1
plot(p.x,p.y,[color '*']);
plot(p(1),p(2),[color '*']);
else
plot(p.x,p.y,color);
plot(p(1),p(2),color);
end
if exist('P', 'var') && (~isempty(P))
gtsam.covarianceEllipse([p.x;p.y],P,color(1));
gtsam.covarianceEllipse([p(1);p(2)],P,color(1));
end

6
matlab/+gtsam/plotPoint3.m
View File

@ -1,12 +1,12 @@
function plotPoint3(p, color, P)
%PLOTPOINT3 Plot a Point3 with an optional covariance matrix
if size(color,2)==1
plot3(p.x,p.y,p.z,[color '*']);
plot3(p(1),p(2),p(3),[color '*']);
else
plot3(p.x,p.y,p.z,color);
plot3(p(1),p(2),p(3),color);
end
if exist('P', 'var')
gtsam.covarianceEllipse3D([p.x;p.y;p.z],P);
gtsam.covarianceEllipse3D([p(1);p(2);p(3)],P);
end
end

2
matlab/+gtsam/plotPose3.m
View File

@ -4,7 +4,7 @@ if nargin<3,axisLength=0.1;end
% get rotation and translation (center)
gRp = pose.rotation().matrix(); % rotation from pose to global
C = pose.translation().vector();
C = pose.translation();
if ~isempty(axisLength)
% draw the camera axes

2
matlab/+gtsam/points2DTrackStereo.m
View File

@ -38,7 +38,7 @@ graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
%% initialize graph and values
initialEstimate = Values;
for i = 1:pointsNum
point_j = points3d{i}.data.retract(0.05*randn(3,1));
point_j = points3d{i}.data + (0.05*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
end

24
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -46,7 +46,7 @@ options.camera.horizon = 60;
% camera baseline
options.camera.baseline = 0.05;
% camera focal length
options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
options.camera.f = round(options.camera.resolution(1) * options.camera.horizon / ...
options.camera.fov);
% camera focal baseline
options.camera.fB = options.camera.f * options.camera.baseline;
@ -54,15 +54,15 @@ options.camera.fB = options.camera.f * options.camera.baseline;
options.camera.disparity = options.camera.fB / options.camera.horizon;
% Monocular Camera Calibration
options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2);
options.camera.resolution(1)/2, options.camera.resolution(2)/2);
% Stereo Camera Calibration
options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
options.camera.resolution(1)/2, options.camera.resolution(2)/2, options.camera.disparity);
% write video output
options.writeVideo = true;
% the testing field size (unit: meter)
options.fieldSize = Point2([100, 100]');
options.fieldSize = Point2(100, 100);
% camera flying speed (unit: meter / second)
options.speed = 20;
% camera flying height
@ -113,14 +113,14 @@ theta = 0;
i = 1;
while i <= cylinderNum
theta = theta + 2*pi/10;
x = 40 * rand * cos(theta) + options.fieldSize.x/2;
y = 20 * rand * sin(theta) + options.fieldSize.y/2;
baseCentroid{i} = Point2([x, y]');
x = 40 * rand * cos(theta) + options.fieldSize(1)/2;
y = 20 * rand * sin(theta) + options.fieldSize(2)/2;
baseCentroid{i} = Point2(x, y);
% prevent two cylinders interact with each other
regenerate = false;
for j = 1:i-1
if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
if i > 1 && norm(baseCentroid{i} - baseCentroid{j}) < options.cylinder.radius * 2
regenerate = true;
break;
end
@ -146,17 +146,17 @@ while 1
angle = 0.1*pi*(rand-0.5);
inc_t = Point3(distance * cos(angle), ...
distance * sin(angle), 0);
t = t.compose(inc_t);
t = t + inc_t;
if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10;
if t(1) > options.fieldSize(1) - 10 || t(2) > options.fieldSize(2) - 10;
break;
end
%t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
% 15, 10]');
camera = PinholeCameraCal3_S2.Lookat(t, ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.camera.monoK);
Point3(options.fieldSize(1)/2, options.fieldSize(2)/2, 0), ...
Point3(0,0,1), options.camera.monoK);
cameraPoses{end+1} = camera.pose;
end

4
matlab/gtsam_examples/MonocularVOExample.m
View File

@ -15,14 +15,14 @@ import gtsam.*
%% Create two cameras and corresponding essential matrix E
aRb = Rot3.Yaw(pi/2);
aTb = Point3 (0.1, 0, 0);
aTb = [.1, 0, 0]';
identity=Pose3;
aPb = Pose3(aRb, aTb);
cameraA = CalibratedCamera(identity);
cameraB = CalibratedCamera(aPb);
%% Create 5 points
P = { Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5) };
P = { [0, 0, 1]', [-0.1, 0, 1]', [0.1, 0, 1]', [0, 0.5, 0.5]', [0, -0.5, 0.5]' };
%% Project points in both cameras
for i=1:5

9
matlab/gtsam_examples/PlanarSLAMExample.m
View File

@ -71,9 +71,12 @@ marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals);
plot2DPoints(result, 'b', marginals);
plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-');
plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-');
plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-');
p_j1 = result.atPoint2(j1);
p_j2 = result.atPoint2(j2);
plot([result.atPose2(i1).x; p_j1(1)],[result.atPose2(i1).y; p_j1(2)], 'c-');
plot([result.atPose2(i2).x; p_j1(1)],[result.atPose2(i2).y; p_j1(2)], 'c-');
plot([result.atPose2(i3).x; p_j2(1)],[result.atPose2(i3).y; p_j2(2)], 'c-');
axis([-0.6 4.8 -1 1])
axis equal
view(2)

11
matlab/gtsam_examples/PlanarSLAMExample_sampling.m
View File

@ -60,15 +60,18 @@ for j=1:2
S{j}=chol(Q{j}); % for sampling
end
plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-');
plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-');
plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-');
p_j1 = sample.atPoint2(j1);
p_j2 = sample.atPoint2(j2);
plot([sample.atPose2(i1).x; p_j1(1)],[sample.atPose2(i1).y; p_j1(2)], 'c-');
plot([sample.atPose2(i2).x; p_j1(1)],[sample.atPose2(i2).y; p_j1(2)], 'c-');
plot([sample.atPose2(i3).x; p_j2(1)],[sample.atPose2(i3).y; p_j2(2)], 'c-');
view(2); axis auto; axis equal
%% Do Sampling on point 2
N=1000;
for s=1:N
delta = S{2}*randn(2,1);
proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2));
proposedPoint = Point2(point{2} + delta);
plotPoint2(proposedPoint,'k.')
end

2
matlab/gtsam_examples/Pose2SLAMwSPCG.m
View File

@ -54,7 +54,7 @@ initialEstimate.print(sprintf('\nInitial estimate:\n'));
%% Optimize using Levenberg-Marquardt optimization with SubgraphSolver
params = gtsam.LevenbergMarquardtParams;
subgraphParams = gtsam.SubgraphSolverParameters;
params.setLinearSolverType('CONJUGATE_GRADIENT');
params.setLinearSolverType('ITERATIVE');
params.setIterativeParams(subgraphParams);
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimize();

4
matlab/gtsam_examples/SBAExample.m
View File

@ -47,7 +47,7 @@ end
%% Add Gaussian priors for a pose and a landmark to constrain the system
cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
firstCamera = PinholeCameraCal3_S2(truth.cameras{1}.pose, truth.K);
graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise));
graph.add(PriorFactorPinholeCameraCal3_S2(symbol('c',1), firstCamera, cameraPriorNoise));
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
@ -64,7 +64,7 @@ for i=1:size(truth.cameras,2)
initialEstimate.insert(symbol('c',i), camera_i);
end
for j=1:size(truth.points,2)
point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
point_j = Point3(truth.points{j} + 0.1*randn(3,1));
initialEstimate.insert(symbol('p',j), point_j);
end
initialEstimate.print(sprintf('\nInitial estimate:\n '));

2
matlab/gtsam_examples/SFMExample.m
View File

@ -58,7 +58,7 @@ for i=1:size(truth.cameras,2)
initialEstimate.insert(symbol('x',i), pose_i);
end
for j=1:size(truth.points,2)
point_j = Point3(truth.points{j}.vector() + 0.1*randn(3,1));
point_j = Point3(truth.points{j} + 0.1*randn(3,1));
initialEstimate.insert(symbol('p',j), point_j);
end
initialEstimate.print(sprintf('\nInitial estimate:\n '));

8
matlab/gtsam_examples/StereoVOExample.m
View File

@ -51,10 +51,10 @@ graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l
initialEstimate = Values;
initialEstimate.insert(x1, first_pose);
% noisy estimate for pose 2
initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)));
initialEstimate.insert(l1, Point3( 1, 1, 5));
initialEstimate.insert(l2, Point3(-1, 1, 5));
initialEstimate.insert(l3, Point3( 0,-.5, 5));
initialEstimate.insert(x2, Pose3(Rot3(), [0.1, -.1, 1.1]'));
initialEstimate.insert(l1, [ 1, 1, 5]');
initialEstimate.insert(l2, [-1, 1, 5]');
initialEstimate.insert(l3, [ 0,-.5, 5]');
%% optimize
fprintf(1,'Optimizing\n'); tic

2
matlab/gtsam_examples/VisualizeMEstimators.m
View File

@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename)
rho = zeros(size(x));
for i = 1:size(x, 2)
w(i) = model.weight(x(i));
rho(i) = model.residual(x(i));
rho(i) = model.loss(x(i));
end
psi = w .* x;

2
matlab/gtsam_tests/testPlanarSLAMExample.m
View File

@ -66,4 +66,4 @@ CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
point_1 = result.atPoint2(symbol('l',1));
marginals.marginalCovariance(symbol('l',1));
CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4));
CHECK('point_1.equals(Point2(2,2),1e-4)',norm(point_1 - Point2(2,2)) < 1e-4);

2
matlab/gtsam_tests/testSFMExample.m
View File

@ -69,5 +69,5 @@ end
for j=1:size(truth.points,2)
point_j = result.atPoint3(symbol('p',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
CHECK('point_j.equals(truth.points{j},1e-5)',norm(point_j - truth.points{j}) < 1e-5)
end

2
matlab/gtsam_tests/testStereoVOExample.m
View File

@ -65,4 +65,4 @@ pose_x1 = result.atPose3(x1);
CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
point_l1 = result.atPoint3(l1);
CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4));
CHECK('point_1.equals(expected_l1,1e-4)',norm(point_l1 - expected_l1) < 1e-4);

8
matlab/gtsam_tests/testValues.m
View File

@ -5,8 +5,8 @@ values = Values;
E = EssentialMatrix(Rot3,Unit3);
tol = 1e-9;
values.insert(0, Point2);
values.insert(1, Point3);
values.insert(0, Point2(0, 0));
values.insert(1, Point3(0, 0, 0));
values.insert(2, Rot2);
values.insert(3, Pose2);
values.insert(4, Rot3);
@ -21,8 +21,8 @@ values.insert(10, imuBias.ConstantBias);
values.insert(11, [1;2;3]);
values.insert(12, [1 2;3 4]);
EXPECT('at',values.atPoint2(0).equals(Point2,tol));
EXPECT('at',values.atPoint3(1).equals(Point3,tol));
EXPECT('at',values.atPoint2(0) == Point2(0, 0));
EXPECT('at',values.atPoint3(1) == Point3(0, 0, 0));
EXPECT('at',values.atRot2(2).equals(Rot2,tol));
EXPECT('at',values.atPose2(3).equals(Pose2,tol));
EXPECT('at',values.atRot3(4).equals(Rot3,tol));

2
matlab/gtsam_tests/testVisualISAMExample.m
View File

@ -51,5 +51,5 @@ end
for j=1:size(truth.points,2)
point_j = result.atPoint3(symbol('l',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
CHECK('point_j.equals(truth.points{j},1e-5)',norm(point_j - truth.points{j}) < 1e-5)
end

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

@ -28,19 +28,19 @@ currentVelocityGlobalIMUbody = currentVelocityGlobal;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
positions(:,1) = currentPoseGlobal.translation;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Trajectory estimate (integrated in the navigation frame)
positionsIMUnav = zeros(3, length(times)+1);
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation;
posesIMUnav(1).p = positionsIMUnav(:,1);
posesIMUnav(1).R = poses(1).R;
% Trajectory estimate (integrated in the body frame)
positionsIMUbody = zeros(3, length(times)+1);
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation;
posesIMUbody(1).p = positionsIMUbody(:,1);
posesIMUbody(1).R = poses(1).R;
@ -120,9 +120,9 @@ for t = times
currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;
positions(:,i) = currentPoseGlobal.translation;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation;
% -
poses(i).p = positions(:,i);
posesIMUbody(i).p = positionsIMUbody(:,i);

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

@ -28,7 +28,7 @@ currentVelocityGlobal = velocity;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
positions(:,1) = currentPoseGlobal.translation;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
@ -112,7 +112,7 @@ for t = times
end
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positions(:,i) = currentPoseGlobal.translation;
i = i + 1;
end

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

@ -7,7 +7,7 @@ measuredOmega = omega1Body;
% Acceleration measurement (in this simple toy example no other forces
% act on the body and the only acceleration is the centripetal Coriolis acceleration)
measuredAcc = Point3(cross(omega1Body, velocity1Body)).vector;
measuredAcc = Point3(cross(omega1Body, velocity1Body));
acc_omega = [ measuredAcc; measuredOmega ];
end

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

@ -6,16 +6,16 @@ import gtsam.*;
% Calculate gyro measured rotation rate by transforming body rotation rate
% into the IMU frame.
measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body)).vector;
measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body));
% Transform both velocities into IMU frame, accounting for the velocity
% induced by rigid body rotation on a lever arm (Coriolis effect).
velocity1inertial = imuFrame.rotation.unrotate( ...
Point3(velocity1Body + cross(omega1Body, imuFrame.translation.vector))).vector;
Point3(velocity1Body + cross(omega1Body, imuFrame.translation)));
imu2in1 = Rot3.Expmap(measuredOmega * deltaT);
velocity2inertial = imu2in1.rotate(imuFrame.rotation.unrotate( ...
Point3(velocity2Body + cross(omega2Body, imuFrame.translation.vector)))).vector;
Point3(velocity2Body + cross(omega2Body, imuFrame.translation))));
% Acceleration in IMU frame
measuredAcc = (velocity2inertial - velocity1inertial) / deltaT;

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

@ -190,13 +190,13 @@ for i = 1:length(times)
newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
% Store data
positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector;
positionsInFixedGT(:,1) = currentPoseFixedGT.translation;
velocityInFixedGT(:,1) = currentVelocityFixedGT;
positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector;
%velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector;
positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector;
positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation;
%velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity;
positionsEstimates(:,i) = currentPoseEstimate.translation;
velocitiesEstimates(:,i) = currentVelocityEstimate;
currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation;
currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix;
else
@ -204,18 +204,18 @@ for i = 1:length(times)
% Update the position and velocity
% x_t = x_0 + v_0*dt + 1/2*a_0*dt^2
% v_t = v_0 + a_0*dt
currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ...
currentPositionFixedGT = Point3(currentPoseFixedGT.translation ...
+ currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT);
currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT;
currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation
% Rotate pose in fixed frame to get pose in rotating frame
previousPositionRotatingGT = currentPoseRotatingGT.translation.vector;
previousPositionRotatingGT = currentPoseRotatingGT.translation;
currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame);
inverseCurrentRotatingFrame = (currentRotatingFrame.inverse);
currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT);
currentPositionRotatingGT = currentPoseRotatingGT.translation.vector;
currentPositionRotatingGT = currentPoseRotatingGT.translation;
% Get velocity in rotating frame by treating it like a position and using compose
% Maybe Luca knows a better way to do this within GTSAM.
@ -230,11 +230,11 @@ for i = 1:length(times)
% - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT;
% Store GT (ground truth) poses
positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector;
positionsInFixedGT(:,i) = currentPoseFixedGT.translation;
velocityInFixedGT(:,i) = currentVelocityFixedGT;
positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector;
positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation;
velocityInRotatingGT(:,i) = currentVelocityRotatingGT;
currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector;
currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation;
currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix;
%% Estimate trajectory in rotating frame using GTSAM (ground truth measurements)
@ -303,9 +303,9 @@ for i = 1:length(times)
currentVelocityEstimate = isam.calculateEstimate(currentVelKey);
currentBias = isam.calculateEstimate(currentBiasKey);
positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
biasEstimates(:,i) = currentBias.vector;
positionsEstimates(:,i) = currentPoseEstimate.translation;
velocitiesEstimates(:,i) = currentVelocityEstimate;
biasEstimates(:,i) = currentBias;
% In matrix form: R_error = R_gt'*R_estimate
% Perform Logmap on the rotation matrix to get a vector

10
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
View File

@ -151,14 +151,14 @@ hold on;
if options.includeCameraFactors
b = [-1000 2000 -2000 2000 -30 30];
for i = 1:size(metadata.camera.gtLandmarkPoints,2)
p = metadata.camera.gtLandmarkPoints(i).vector;
p = metadata.camera.gtLandmarkPoints(i);
if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6))
plot3(p(1), p(2), p(3), 'k+');
end
end
pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0));
for i = 1:length(pointsToPlot)
p = pointsToPlot(i).vector;
p = pointsToPlot(i);
plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10);
end
end
@ -233,9 +233,9 @@ for k=1:numMonteCarloRuns
for i=0:options.trajectoryLength
% compute estimation errors
currentPoseKey = symbol('x', i);
gtPosition = gtValues.at(currentPoseKey).translation.vector;
estPosition = estimate.at(currentPoseKey).translation.vector;
estR = estimate.at(currentPoseKey).rotation.matrix;
gtPosition = gtValues.atPose3(currentPoseKey).translation;
estPosition = estimate.atPose3(currentPoseKey).translation;
estR = estimate.atPose3(currentPoseKey).rotation.matrix;
errPosition = estPosition - gtPosition;
% compute covariances:

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

@ -14,7 +14,7 @@ graph = NonlinearFactorGraph;
for i=0:length(measurements)
% Get the current pose
currentPoseKey = symbol('x', i);
currentPose = values.at(currentPoseKey);
currentPose = values.atPose3(currentPoseKey);
if i==0
%% first time step, add priors
@ -26,11 +26,11 @@ for i=0:length(measurements)
% IMU velocity and bias priors
if options.includeIMUFactors == 1
currentVelKey = symbol('v', 0);
currentVel = values.at(currentVelKey).vector;
currentVel = values.atPoint3(currentVelKey);
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
currentBiasKey = symbol('b', 0);
currentBias = values.at(currentBiasKey);
currentBias = values.atPoint3(currentBiasKey);
graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias));
end
@ -155,7 +155,7 @@ for i=0:length(measurements)
if options.includeCameraFactors == 1
for j = 1:length(measurements(i).landmarks)
cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1);
cameraPixelMeasurement = measurements(i).landmarks(j).vector;
cameraPixelMeasurement = measurements(i).landmarks(j);
% Only add the measurement if it is in the image frame (based on calibration)
if(cameraPixelMeasurement(1) > 0 && cameraPixelMeasurement(2) > 0 ...
&& cameraPixelMeasurement(1) < 2*metadata.camera.calibration.px ...

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

@ -40,7 +40,7 @@ if options.useRealData == 1
%% gt Between measurements
if options.includeBetweenFactors == 1 && i > 0
prevPose = values.at(currentPoseKey - 1);
prevPose = values.atPose3(currentPoseKey - 1);
deltaPose = prevPose.between(currentPose);
measurements(i).deltaVector = Pose3.Logmap(deltaPose);
end
@ -65,7 +65,7 @@ if options.useRealData == 1
IMUdeltaPose = IMUPose1.between(IMUPose2);
IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose);
IMUdeltaRotVector = IMUdeltaPoseVector(1:3);
IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame
IMUdeltaPositionVector = IMUPose2.translation - IMUPose1.translation; % translation in absolute frame
measurements(i).imu(j).deltaT = deltaT;
@ -88,7 +88,7 @@ if options.useRealData == 1
%% gt GPS measurements
if options.includeGPSFactors == 1 && i > 0
gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector;
gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation;
measurements(i).gpsPositionVector = gpsPositionVector;
end

4
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m
View File

@ -5,9 +5,9 @@ function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory( ...
import gtsam.*;
imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3))).vector;
accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3)));
finalPosition = Point3(initialPoseGlobal.translation.vector ...
finalPosition = Point3(initialPoseGlobal.translation ...
+ initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
finalRotation = initialPoseGlobal.rotation.compose(imu2in1);

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

@ -3,7 +3,7 @@ function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_bodyFrame(
% Before integrating in the body frame we need to compensate for the Coriolis
% effect
acc_body = acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body)).vector;
acc_body = acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body));
% after compensating for coriolis this will be essentially zero
% since we are moving at constant body velocity
@ -16,8 +16,8 @@ finalPositionBody = velocity1Body * deltaT + 0.5 * acc_body * deltaT * deltaT;
finalVelocityBody = velocity1Body + acc_body * deltaT;
%% Express the integrated quantities in the global frame
finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)).vector() );
finalPosition = initialPoseGlobal.translation().vector() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)).vector() ;
finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)) );
finalPosition = initialPoseGlobal.translation() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)) ;
finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
% Include position and rotation in a pose
finalPose = Pose3(finalRotation, Point3(finalPosition) );

4
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m
View File

@ -9,8 +9,8 @@ finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
intermediateRotation = initialPoseGlobal.rotation.compose( Rot3.Expmap(acc_omegaIMU(4:6) * deltaT/2 ));
% Integrate positions (equation (1) in Lupton)
accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3))).vector;
finalPosition = Point3(initialPoseGlobal.translation.vector ...
accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3)));
finalPosition = Point3(initialPoseGlobal.translation ...
+ initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;

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

@ -6,16 +6,16 @@ import gtsam.*;
% Rotation: R^1_2
body2in1 = Rot3.Expmap(omega1Body * deltaT);
% Velocity 2 in frame 1: v^1_2 = R^1_2 v^2_2
velocity2inertial = body2in1.rotate(Point3(velocity2Body)).vector;
velocity2inertial = body2in1.rotate(Point3(velocity2Body));
% Acceleration: a^1 = (v^1_2 - v^1_1)/dt
accelBody1 = (velocity2inertial - velocity1Body) / deltaT;
% Velocity 1 in frame W: v^W_1 = R^W_1 v^1_1
initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body)).vector;
initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body));
% Acceleration in frame W: a^W = R^W_1 a^1
accelGlobal = initialPose.rotation().rotate(Point3(accelBody1)).vector;
accelGlobal = initialPose.rotation().rotate(Point3(accelBody1));
finalPosition = Point3(initialPose.translation.vector + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalPosition = Point3(initialPose.translation + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
finalRotation = initialPose.rotation.compose(body2in1);
finalPose = Pose3(finalRotation, finalPosition);

4
matlab/unstable_examples/+imuSimulator/test1onestep.m
View File

@ -10,7 +10,7 @@ omega = [0;0;0.1];
velocity = [1;0;0];
R = Rot3.Expmap(omega * deltaT);
velocity2body = R.unrotate(Point3(velocity)).vector;
velocity2body = R.unrotate(Point3(velocity));
acc_omegaExpected = [-0.01; 0; 0; 0; 0; 0.1];
acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity2body, deltaT, Pose3(Rot3, Point3([1;0;0])));
disp('IMU measurement discrepancy:');
@ -40,7 +40,7 @@ disp(acc_omegaActual - acc_omegaExpected);
initialPose = Pose3;
initialVelocity = velocity;
finalPoseExpected = Pose3.Expmap([ omega; velocity ] * deltaT);
finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity)).vector;
finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity));
[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity, deltaT);
disp('Final pose discrepancy:');
disp(finalPoseExpected.between(finalPoseActual).matrix);

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

@ -21,12 +21,12 @@ positions = zeros(3, length(times)+1);
i = 2;
for t = times
velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal)).vector;
velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal));
R = Rot3.Expmap(omega * deltaT);
velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal)).vector;
velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal));
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory(currentPoseGlobal, omega, velocity1body, velocity2body, deltaT);
positions(:,i) = currentPoseGlobal.translation.vector;
positions(:,i) = currentPoseGlobal.translation;
i = i + 1;
end

16
matlab/unstable_examples/+imuSimulator/test3constbody.m
View File

@ -26,27 +26,27 @@ currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
% Initial state (IMU)
currentPoseGlobalIMU = Pose3; %currentPoseGlobal.compose(IMUinBody);
%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)).vector; % no Coriolis here?
%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)); % no Coriolis here?
currentVelocityGlobalIMU = IMUinBody.rotation.unrotate( ...
Point3(velocity + cross(omega, IMUinBody.translation.vector))).vector;
Point3(velocity + cross(omega, IMUinBody.translation)));
% Positions
% body trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
positions(:,1) = currentPoseGlobal.translation;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Expected IMU trajectory (from body trajectory and lever arm)
positionsIMUe = zeros(3, length(times)+1);
positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation;
posesIMUe(1).p = positionsIMUe(:,1);
posesIMUe(1).R = poses(1).R * IMUinBody.rotation.matrix;
% Integrated IMU trajectory (from IMU measurements)
positionsIMU = zeros(3, length(times)+1);
positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation;
posesIMU(1).p = positionsIMU(:,1);
posesIMU(1).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix;
@ -62,9 +62,9 @@ for t = times
currentPoseGlobalIMU, currentVelocityGlobalIMU, acc_omega, deltaT);
% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUe(:,i) = currentPoseGlobal.translation.vector + currentPoseGlobal.rotation.matrix * IMUinBody.translation.vector;
positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
positions(:,i) = currentPoseGlobal.translation;
positionsIMUe(:,i) = currentPoseGlobal.translation + currentPoseGlobal.rotation.matrix * IMUinBody.translation;
positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation;
poses(i).p = positions(:,i);
posesIMUe(i).p = positionsIMUe(:,i);

12
matlab/unstable_examples/+imuSimulator/test4circle.m
View File

@ -34,19 +34,19 @@ currentVelocityGlobalIMUbody = currentVelocityGlobal;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
positions(:,1) = currentPoseGlobal.translation;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Trajectory estimate (integrated in the navigation frame)
positionsIMUnav = zeros(3, length(times)+1);
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation;
posesIMUnav(1).p = positionsIMUnav(:,1);
posesIMUnav(1).R = poses(1).R;
% Trajectory estimate (integrated in the body frame)
positionsIMUbody = zeros(3, length(times)+1);
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation;
posesIMUbody(1).p = positionsIMUbody(:,1);
posesIMUbody(1).R = poses(1).R;
@ -72,9 +72,9 @@ for t = times
currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;
positions(:,i) = currentPoseGlobal.translation;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation;
% -
poses(i).p = positions(:,i);
posesIMUbody(i).p = positionsIMUbody(:,i);

25
matlab/unstable_examples/FlightCameraTransformIMU.m
View File

@ -120,7 +120,7 @@ for i=1:size(trajectory)-1
end
% current ground-truth position indicator
h_cursor = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'*');
h_cursor = plot3(a1, pose_t(1),pose_t(2),pose_t(3),'*');
camera_pose = pose.compose(camera_transform);
@ -198,7 +198,7 @@ for i=1:size(trajectory)-1
if ~result.exists(lKey)
p = landmarks.atPoint3(lKey);
n = normrnd(0,landmark_noise,3,1);
noisy_landmark = Point3(p.x()+n(1),p.y()+n(2),p.z()+n(3));
noisy_landmark = p + n;
initial.insert(lKey, noisy_landmark);
% and add a prior since its position is known
@ -245,32 +245,33 @@ for i=1:size(trajectory)-1
initial = Values;
fg = NonlinearFactorGraph;
currentVelocityGlobal = result.at(currentVelKey);
currentBias = result.at(currentBiasKey);
currentVelocityGlobal = result.atPoint3(currentVelKey);
currentBias = result.atConstantBias(currentBiasKey);
%% plot current pose result
isam_pose = result.at(xKey);
isam_pose = result.atPose3(xKey);
pose_t = isam_pose.translation();
if exist('h_result','var')
delete(h_result);
end
h_result = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'^b', 'MarkerSize', 10);
h_result = plot3(a1, pose_t(1),pose_t(2),pose_t(3),'^b', 'MarkerSize', 10);
title(a1, sprintf('Step %d', i));
if exist('h_text1(1)', 'var')
delete(h_text1(1));
% delete(h_text2(1));
end
ty = result.at(transformKey).translation().y();
K_estimate = result.at(calibrationKey);
t = result.atPose3(transformKey).translation();
ty = t(2);
K_estimate = result.atCal3_S2(calibrationKey);
K_errors = K.localCoordinates(K_estimate);
camera_transform_estimate = result.at(transformKey);
camera_transform_estimate = result.atPose3(transformKey);
fx = result.at(calibrationKey).fx();
fy = result.at(calibrationKey).fy();
fx = result.atCal3_S2(calibrationKey).fx();
fy = result.atCal3_S2(calibrationKey).fy();
% h_text1 = text(-600,0,0,sprintf('Y-Transform(0.0): %0.2f',ty));
text(0,1300,0,sprintf('Calibration and IMU-cam transform errors:'));
@ -304,7 +305,7 @@ for i=1:size(trajectory)-1
end
%% print out final camera transform and write video
result.at(transformKey);
result.atPose3(transformKey);
if(write_video)
close(videoObj);
end

46
matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
View File

@ -53,7 +53,7 @@ y_shift = Point3(0,0.5,0);
% insert shifted points
for i=1:nrPoints
initial.insert(100+i,landmarks{i}.compose(y_shift));
initial.insert(100+i,landmarks{i} + y_shift);
end
figure(1);
@ -134,7 +134,7 @@ for i=1:steps
end
if i == 2
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
end
if i > 1
@ -144,7 +144,7 @@ for i=1:steps
step = move_circle;
end
initial.insert(i,result.at(i-1).compose(step));
initial.insert(i,result.atPose3(i-1).compose(step));
fg.add(BetweenFactorPose3(i-1,i, step, covariance));
deltaT = 1;
@ -158,10 +158,13 @@ for i=1:steps
[ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentIMUPoseGlobal, omega, velocity, velocity, deltaT);
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
params = gtsam.PreintegrationParams.MakeSharedD(9.81);
params.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3));
params.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3));
params.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3));
currentSummarizedMeasurement = gtsam.PreintegratedImuMeasurements( ...
params, currentBias);
accMeas = acc_omega(1:3)-g;
omegaMeas = acc_omega(4:6);
@ -171,7 +174,7 @@ for i=1:steps
fg.add(ImuFactor( ...
i-1, currentVelKey-1, ...
i, currentVelKey, ...
currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
currentBiasKey, currentSummarizedMeasurement));
% Bias evolution as given in the IMU metadata
fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
@ -204,8 +207,8 @@ for i=1:steps
initial = Values;
fg = NonlinearFactorGraph;
currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
currentBias = isam.calculateEstimate(currentBiasKey);
currentVelocityGlobal = isam.calculateEstimate().atVector(currentVelKey);
currentBias = isam.calculateEstimate().atConstantBias(currentBiasKey);
%% Compute some marginals
marginal = isam.marginalCovariance(calibrationKey);
@ -249,10 +252,10 @@ for i=1:steps
gtsam.plotPose3(currentIMUPoseGlobal, [], 2);
%% plot results
result_camera_transform = result.at(transformKey);
result_camera_transform = result.atPose3(transformKey);
for j=1:i
gtsam.plotPose3(result.at(j),[],0.5);
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
gtsam.plotPose3(result.atPose3(j),[],0.5);
gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5);
end
xlabel('x (m)');
@ -265,14 +268,15 @@ for i=1:steps
% axis equal
for l=101:100+nrPoints
plotPoint3(result.at(l),'g');
plotPoint3(result.atPoint3(l),'g');
end
ty = result.at(transformKey).translation().y();
fx = result.at(calibrationKey).fx();
fy = result.at(calibrationKey).fy();
px = result.at(calibrationKey).px();
py = result.at(calibrationKey).py();
t = result.atPose3(transformKey).translation();
ty = t(2);
fx = result.atCal3_S2(calibrationKey).fx();
fy = result.atCal3_S2(calibrationKey).fy();
px = result.atCal3_S2(calibrationKey).px();
py = result.atCal3_S2(calibrationKey).py();
text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
text(1,5,3,sprintf('fx(900): %.0f',fx));
text(1,5,1,sprintf('fy(900): %.0f',fy));
@ -342,10 +346,10 @@ end
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
disp('Transform after optimization');
result.at(transformKey)
result.atPose3(transformKey)
disp('Calibration after optimization');
result.at(calibrationKey)
result.atCal3_S2(calibrationKey)
disp('Bias after optimization');
currentBias

6
matlab/unstable_examples/plot_projected_landmarks.m
View File

@ -25,9 +25,9 @@ for i = 0:measurement_keys.size-1
key_index = gtsam.symbolIndex(key);
p = landmarks.atPoint3(gtsam.symbol('l',key_index));
x(i+1) = p.x;
y(i+1) = p.y;
z(i+1) = p.z;
x(i+1) = p(1);
y(i+1) = p(2);
z(i+1) = p(3);
end

4
matlab/unstable_examples/project_landmarks.m
View File

@ -11,9 +11,9 @@ function [ measurements ] = project_landmarks( pose, landmarks, K )
z = camera.project(landmarks.atPoint3(symbol('l',i)));
% check bounding box
if z.x < 0 || z.x > 1280
if z(1) < 0 || z(1) > 1280
continue
elseif z.y < 0 || z.y > 960
elseif z(2) < 0 || z(2) > 960
continue
end