diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 2d766dc6a..10d582287 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -40,20 +40,6 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { -Vector callEvaluateError(const ImuFactor& factor, - const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, - const imuBias::ConstantBias& bias) -{ - return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); -} - -Rot3 evaluateRotationError(const ImuFactor& factor, - const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, - const imuBias::ConstantBias& bias) -{ - return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; -} - ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, @@ -108,17 +94,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation( measuredAccs, measuredOmegas, deltaTs).deltaRij; } -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) -{ - return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); -} - - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) -{ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); -} - } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index c8357aba4..a063244a3 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -125,9 +125,6 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2) SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05))); - double prior_outlier = 0.01; - double prior_inlier = 0.99; - gtsam::Values valA, valB; valA.insert(keyA, orgA_T_currA); valB.insert(keyB, orgB_T_currB);