41 lines
1.3 KiB
Matlab
41 lines
1.3 KiB
Matlab
% test wrapping of Values
|
|
import gtsam.*;
|
|
|
|
values = Values;
|
|
E = EssentialMatrix(Rot3,Unit3);
|
|
tol = 1e-9;
|
|
|
|
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);
|
|
values.insert(5, Pose3);
|
|
values.insert(6, Cal3_S2);
|
|
values.insert(7, Cal3DS2);
|
|
values.insert(8, Cal3Bundler);
|
|
values.insert(9, E);
|
|
values.insert(10, imuBias.ConstantBias);
|
|
|
|
% special cases for Vector and Matrix:
|
|
values.insert(11, [1;2;3]);
|
|
values.insert(12, [1 2;3 4]);
|
|
|
|
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));
|
|
EXPECT('at',values.atPose3(5).equals(Pose3,tol));
|
|
EXPECT('at',values.atCal3_S2(6).equals(Cal3_S2,tol));
|
|
EXPECT('at',values.atCal3DS2(7).equals(Cal3DS2,tol));
|
|
EXPECT('at',values.atCal3Bundler(8).equals(Cal3Bundler,tol));
|
|
EXPECT('at',values.atEssentialMatrix(9).equals(E,tol));
|
|
EXPECT('at',values.atConstantBias(10).equals(imuBias.ConstantBias,tol));
|
|
|
|
% special cases for Vector and Matrix:
|
|
actualVector = values.atVector(11);
|
|
EQUALITY('at',[1;2;3],actualVector,tol);
|
|
actualMatrix = values.atMatrix(12);
|
|
EQUALITY('at',[1 2;3 4],actualMatrix,tol);
|