From 840831b62bda4248b41615fc74974f59e50e95cc Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 17 Aug 2020 14:37:12 -0400 Subject: [PATCH] Ported MATLAB Examples --- matlab/+gtsam/Point2.m | 12 +++++ matlab/+gtsam/Point3.m | 12 +++++ matlab/+gtsam/cylinderSampleProjection.m | 6 +-- .../+gtsam/cylinderSampleProjectionStereo.m | 20 ++++---- matlab/+gtsam/cylinderSampling.m | 6 +-- matlab/+gtsam/plotCamera.m | 2 +- matlab/+gtsam/plotFlyingResults.m | 16 +++---- matlab/+gtsam/plotPoint2.m | 6 +-- matlab/+gtsam/plotPoint3.m | 6 +-- matlab/+gtsam/plotPose3.m | 2 +- matlab/+gtsam/points2DTrackStereo.m | 2 +- matlab/gtsam_examples/CameraFlyingExample.m | 24 +++++----- matlab/gtsam_examples/MonocularVOExample.m | 4 +- matlab/gtsam_examples/PlanarSLAMExample.m | 9 ++-- .../PlanarSLAMExample_sampling.m | 11 +++-- matlab/gtsam_examples/Pose2SLAMwSPCG.m | 2 +- matlab/gtsam_examples/SBAExample.m | 4 +- matlab/gtsam_examples/SFMExample.m | 2 +- matlab/gtsam_examples/StereoVOExample.m | 8 ++-- matlab/gtsam_examples/VisualizeMEstimators.m | 2 +- matlab/gtsam_tests/testPlanarSLAMExample.m | 2 +- matlab/gtsam_tests/testSFMExample.m | 2 +- matlab/gtsam_tests/testStereoVOExample.m | 2 +- matlab/gtsam_tests/testValues.m | 8 ++-- matlab/gtsam_tests/testVisualISAMExample.m | 2 +- .../+imuSimulator/IMUComparison.m | 12 ++--- .../+imuSimulator/IMUComparison_with_cov.m | 4 +- .../+imuSimulator/calculateIMUMeas_coriolis.m | 2 +- .../+imuSimulator/calculateIMUMeasurement.m | 6 +-- .../+imuSimulator/coriolisExample.m | 30 ++++++------ .../+imuSimulator/covarianceAnalysisBetween.m | 10 ++-- .../covarianceAnalysisCreateFactorGraph.m | 8 ++-- .../covarianceAnalysisCreateTrajectory.m | 6 +-- .../+imuSimulator/integrateIMUTrajectory.m | 4 +- .../integrateIMUTrajectory_bodyFrame.m | 6 +-- .../integrateIMUTrajectory_navFrame.m | 4 +- .../+imuSimulator/integrateTrajectory.m | 8 ++-- .../+imuSimulator/test1onestep.m | 4 +- .../+imuSimulator/test2constglobal.m | 6 +-- .../+imuSimulator/test3constbody.m | 16 +++---- .../+imuSimulator/test4circle.m | 12 ++--- .../FlightCameraTransformIMU.m | 25 +++++----- ...ansformCalProjectionFactorIMUExampleISAM.m | 46 ++++++++++--------- .../plot_projected_landmarks.m | 6 +-- matlab/unstable_examples/project_landmarks.m | 4 +- 45 files changed, 213 insertions(+), 178 deletions(-) create mode 100644 matlab/+gtsam/Point2.m create mode 100644 matlab/+gtsam/Point3.m diff --git a/matlab/+gtsam/Point2.m b/matlab/+gtsam/Point2.m new file mode 100644 index 000000000..3ea65c33e --- /dev/null +++ b/matlab/+gtsam/Point2.m @@ -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 \ No newline at end of file diff --git a/matlab/+gtsam/Point3.m b/matlab/+gtsam/Point3.m new file mode 100644 index 000000000..5f66b4517 --- /dev/null +++ b/matlab/+gtsam/Point3.m @@ -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 \ No newline at end of file diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 2b913b52d..697a57faa 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -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 diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 10409ac6d..58b4140fd 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -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 diff --git a/matlab/+gtsam/cylinderSampling.m b/matlab/+gtsam/cylinderSampling.m index dcaa9c721..dc76295fa 100644 --- a/matlab/+gtsam/cylinderSampling.m +++ b/matlab/+gtsam/cylinderSampling.m @@ -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 \ No newline at end of file diff --git a/matlab/+gtsam/plotCamera.m b/matlab/+gtsam/plotCamera.m index d0d1f45b7..986cd9a68 100644 --- a/matlab/+gtsam/plotCamera.m +++ b/matlab/+gtsam/plotCamera.m @@ -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; diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m index 5d4a287c4..202f2409b 100644 --- a/matlab/+gtsam/plotFlyingResults.m +++ b/matlab/+gtsam/plotFlyingResults.m @@ -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 diff --git a/matlab/+gtsam/plotPoint2.m b/matlab/+gtsam/plotPoint2.m index cd066951d..8d10ecab6 100644 --- a/matlab/+gtsam/plotPoint2.m +++ b/matlab/+gtsam/plotPoint2.m @@ -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 \ No newline at end of file diff --git a/matlab/+gtsam/plotPoint3.m b/matlab/+gtsam/plotPoint3.m index 390b44939..85c84a210 100644 --- a/matlab/+gtsam/plotPoint3.m +++ b/matlab/+gtsam/plotPoint3.m @@ -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 diff --git a/matlab/+gtsam/plotPose3.m b/matlab/+gtsam/plotPose3.m index 8c3c7dd76..258e3f776 100644 --- a/matlab/+gtsam/plotPose3.m +++ b/matlab/+gtsam/plotPose3.m @@ -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 diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 60c9f1295..04cddb7f7 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -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 diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index add2bc75a..a0dfef22a 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -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 diff --git a/matlab/gtsam_examples/MonocularVOExample.m b/matlab/gtsam_examples/MonocularVOExample.m index 11c4253de..0d09a1487 100644 --- a/matlab/gtsam_examples/MonocularVOExample.m +++ b/matlab/gtsam_examples/MonocularVOExample.m @@ -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 diff --git a/matlab/gtsam_examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m index aec933d74..3febc23e6 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample.m +++ b/matlab/gtsam_examples/PlanarSLAMExample.m @@ -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) diff --git a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m index 375ed645c..93979a68a 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m @@ -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 \ No newline at end of file diff --git a/matlab/gtsam_examples/Pose2SLAMwSPCG.m b/matlab/gtsam_examples/Pose2SLAMwSPCG.m index 67f22fe1d..2df7efe2f 100644 --- a/matlab/gtsam_examples/Pose2SLAMwSPCG.m +++ b/matlab/gtsam_examples/Pose2SLAMwSPCG.m @@ -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(); diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index 584ace09a..f94a2ee4e 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -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 ')); diff --git a/matlab/gtsam_examples/SFMExample.m b/matlab/gtsam_examples/SFMExample.m index 6700e90d2..a57da929c 100644 --- a/matlab/gtsam_examples/SFMExample.m +++ b/matlab/gtsam_examples/SFMExample.m @@ -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 ')); diff --git a/matlab/gtsam_examples/StereoVOExample.m b/matlab/gtsam_examples/StereoVOExample.m index b437ca80c..e7345fcf2 100644 --- a/matlab/gtsam_examples/StereoVOExample.m +++ b/matlab/gtsam_examples/StereoVOExample.m @@ -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 diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index 8a0485334..ce505df5d 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -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; diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m index d3cab5d1f..e0b4dbfc8 100644 --- a/matlab/gtsam_tests/testPlanarSLAMExample.m +++ b/matlab/gtsam_tests/testPlanarSLAMExample.m @@ -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); diff --git a/matlab/gtsam_tests/testSFMExample.m b/matlab/gtsam_tests/testSFMExample.m index 985cbdb2c..a1f63c3a7 100644 --- a/matlab/gtsam_tests/testSFMExample.m +++ b/matlab/gtsam_tests/testSFMExample.m @@ -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 diff --git a/matlab/gtsam_tests/testStereoVOExample.m b/matlab/gtsam_tests/testStereoVOExample.m index 218d0ace1..c721244ba 100644 --- a/matlab/gtsam_tests/testStereoVOExample.m +++ b/matlab/gtsam_tests/testStereoVOExample.m @@ -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)); \ No newline at end of file +CHECK('point_1.equals(expected_l1,1e-4)',norm(point_l1 - expected_l1) < 1e-4); \ No newline at end of file diff --git a/matlab/gtsam_tests/testValues.m b/matlab/gtsam_tests/testValues.m index fe2cd30fe..48bc83f2c 100644 --- a/matlab/gtsam_tests/testValues.m +++ b/matlab/gtsam_tests/testValues.m @@ -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)); diff --git a/matlab/gtsam_tests/testVisualISAMExample.m b/matlab/gtsam_tests/testVisualISAMExample.m index 223e823a6..f75942ea7 100644 --- a/matlab/gtsam_tests/testVisualISAMExample.m +++ b/matlab/gtsam_tests/testVisualISAMExample.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m index 68e20bb25..871f023ef 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -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); diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m index c589bea32..450697de0 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m index c86e40a21..0d8abad2c 100644 --- a/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m index 534b9365e..5ed1fc516 100644 --- a/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m @@ -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; diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 35d27aa73..ee4deb433 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 2eddf75ee..64ba36d3b 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -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: diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 00ae4b9c2..07f146dcb 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -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 ... diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 195b7ff69..3d8a9b5d2 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m index 3f72f1215..2de1e1103 100644 --- a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m @@ -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); diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m index 50b223060..bec2d760d 100644 --- a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m @@ -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) ); diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m index b919520ac..ea851315f 100644 --- a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m @@ -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; diff --git a/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m index e51b622b0..a704342ae 100644 --- a/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m @@ -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); diff --git a/matlab/unstable_examples/+imuSimulator/test1onestep.m b/matlab/unstable_examples/+imuSimulator/test1onestep.m index 883569849..cb66d23d6 100644 --- a/matlab/unstable_examples/+imuSimulator/test1onestep.m +++ b/matlab/unstable_examples/+imuSimulator/test1onestep.m @@ -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); diff --git a/matlab/unstable_examples/+imuSimulator/test2constglobal.m b/matlab/unstable_examples/+imuSimulator/test2constglobal.m index 19956d08a..6ab35d50b 100644 --- a/matlab/unstable_examples/+imuSimulator/test2constglobal.m +++ b/matlab/unstable_examples/+imuSimulator/test2constglobal.m @@ -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 diff --git a/matlab/unstable_examples/+imuSimulator/test3constbody.m b/matlab/unstable_examples/+imuSimulator/test3constbody.m index b3ee2edfc..8ee14ab78 100644 --- a/matlab/unstable_examples/+imuSimulator/test3constbody.m +++ b/matlab/unstable_examples/+imuSimulator/test3constbody.m @@ -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); diff --git a/matlab/unstable_examples/+imuSimulator/test4circle.m b/matlab/unstable_examples/+imuSimulator/test4circle.m index 22ee175dd..ab2c546db 100644 --- a/matlab/unstable_examples/+imuSimulator/test4circle.m +++ b/matlab/unstable_examples/+imuSimulator/test4circle.m @@ -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); diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index 9a8a27344..d2f2bc34d 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -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 \ No newline at end of file diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index 4557d711f..9796a9737 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -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 diff --git a/matlab/unstable_examples/plot_projected_landmarks.m b/matlab/unstable_examples/plot_projected_landmarks.m index 6b8101844..30e222016 100644 --- a/matlab/unstable_examples/plot_projected_landmarks.m +++ b/matlab/unstable_examples/plot_projected_landmarks.m @@ -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 diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m index aaccc9248..3bccef94b 100644 --- a/matlab/unstable_examples/project_landmarks.m +++ b/matlab/unstable_examples/project_landmarks.m @@ -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