diff --git a/matlab/gtsam_tests/testCal3Unified.m b/matlab/gtsam_tests/testCal3Unified.m index 498c65343..ec5bff871 100644 --- a/matlab/gtsam_tests/testCal3Unified.m +++ b/matlab/gtsam_tests/testCal3Unified.m @@ -5,3 +5,8 @@ K = Cal3Unified; EXPECT('fx',K.fx()==1); EXPECT('fy',K.fy()==1); +params = PreintegrationParams.MakeSharedU(-9.81); +%params.getOmegaCoriolis() + +expectedBodyPSensor = gtsam.Pose3(gtsam.Rot3(0, 0, 0, 0, 0, 0, 0, 0, 0), gtsam.Point3(0, 0, 0)); +EXPECT('getBodyPSensor', expectedBodyPSensor.equals(params.getBodyPSensor(), 1e-9));