diff --git a/tracking/orientation_tracker.cc b/tracking/orientation_tracker.cc index ac20e967281..4f381c895f1 100644 --- a/tracking/orientation_tracker.cc +++ b/tracking/orientation_tracker.cc @@ -89,7 +89,7 @@ Vector4 OrientationTracker::OnGyroscopeData(const GyroscopeData& event) sensor_fusion_->ProcessGyroscopeSample(data); - return OrientationTracker::GetPose(data.sensor_timestamp_ns + sampling_period_ns_); + return GetPose(data.sensor_timestamp_ns + sampling_period_ns_); } } // namespace cardboard diff --git a/tracking/sensors/sensor_fusion_ekf.cc b/tracking/sensors/sensor_fusion_ekf.cc index 575dde6f00a..dabd788ef4d 100644 --- a/tracking/sensors/sensor_fusion_ekf.cc +++ b/tracking/sensors/sensor_fusion_ekf.cc @@ -59,13 +59,14 @@ namespace { // angle = norm(a) // axis = a.normalized() // If norm(a) == 0, it returns an identity rotation. - static inline Rotation RotationFromVector(const Vector3& a) + static inline void RotationFromVector(const Vector3& a, Rotation& r) { const double norm_a = Length(a); if (norm_a < kEpsilon) { - return Rotation::Identity(); + r = Rotation::Identity(); + return; } - return Rotation::FromAxisAndAngle(a / norm_a, norm_a); + r = Rotation::FromAxisAndAngle(a / norm_a, norm_a); } } // namespace @@ -199,7 +200,8 @@ void SensorFusionEkf::ComputeMeasurementJacobian() Vector3 delta = Vector3::Zero(); delta[dof] = kFiniteDifferencingEpsilon; - const Rotation epsilon_rotation = RotationFromVector(delta); + Rotation epsilon_rotation; + RotationFromVector(delta, epsilon_rotation); const Vector3 delta_rotation = ComputeInnovation(epsilon_rotation * current_state_.sensor_from_start_rotation); @@ -263,7 +265,8 @@ void SensorFusionEkf::ProcessAccelerometerSample(const AccelerometerData& sample * state_covariance_; // Updates pose and associate covariance matrix. - const Rotation rotation_from_state_update = RotationFromVector(state_update_); + Rotation rotation_from_state_update; + RotationFromVector(state_update_, rotation_from_state_update); current_state_.sensor_from_start_rotation = rotation_from_state_update * current_state_.sensor_from_start_rotation;