diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m index 79a96209d..f6a896cff 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExample.m +++ b/matlab/unstable_examples/TransformProjectionFactorExample.m @@ -33,7 +33,7 @@ y_shift = Point3(0,1,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); diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m index ca5b70c62..88f6cc63c 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m @@ -47,7 +47,7 @@ y_shift = Point3(0,1,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); @@ -146,7 +146,8 @@ for i=1:20 plotPoint3(result.atPoint3(l),'g'); end - ty = result.atPose3(1000).translation().y(); + t = result.atPose3(1000).translation(); + ty = t(2); text(5,5,5,sprintf('Y-Transform: %0.2g',ty)); if(write_video) diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m index 5cfd0aa80..2d1de5855 100644 --- a/matlab/unstable_examples/testTSAMFactors.m +++ b/matlab/unstable_examples/testTSAMFactors.m @@ -50,8 +50,8 @@ result = optimizer.optimize(); % Check result CHECK('error',result.atPose2(100).equals(b1,1e-5)) CHECK('error',result.atPose2(10).equals(origin,1e-5)) -CHECK('error',result.atPoint2(1).equals(Point2(0,1),1e-5)) -CHECK('error',result.atPoint2(2).equals(Point2(0,1),1e-5)) +CHECK('error',result.atPoint2(1) - Point2(0,1) < 1e-5) +CHECK('error',result.atPoint2(2) - Point2(0,1) < 1e-5) CHECK('error',result.atPose2(20).equals(origin,1e-5)) CHECK('error',result.atPose2(200).equals(b2,1e-5))