From 0fafffb02e9ac536244f1db2e1aa1b334a7e937d Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 2 Mar 2015 17:04:19 -0500 Subject: [PATCH] Made functional correctMeasurementsByBiasAndSensorPose --- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/ImuFactor.cpp | 2 +- gtsam/navigation/PreintegrationBase.h | 8 +++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1c22bab9a..edb222261 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -78,7 +78,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor); const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 81120b7c6..4adf324c7 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -67,7 +67,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + boost::tie(correctedAcc, correctedOmega) = correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, body_P_sensor); const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 29b9b6e66..08c16846a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -180,9 +180,10 @@ public: update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); } - void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, boost::optional body_P_sensor) { + boost::tuple + correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, + const Vector3& measuredOmega, boost::optional body_P_sensor) { + Vector3 correctedAcc, correctedOmega; correctedAcc = biasHat_.correctAccelerometer(measuredAcc); correctedOmega = biasHat_.correctGyroscope(measuredOmega); @@ -195,6 +196,7 @@ public: correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); // linear acceleration vector in the body frame } + return boost::make_tuple(correctedAcc, correctedOmega); } /// Predict state at time j