Explorar o código

Fix Rotation return value

A really weird quirk of Flipper build system, or something...
Ivan Podogov %!s(int64=2) %!d(string=hai) anos
pai
achega
06b91a0c54
Modificáronse 2 ficheiros con 9 adicións e 6 borrados
  1. 1 1
      tracking/orientation_tracker.cc
  2. 8 5
      tracking/sensors/sensor_fusion_ekf.cc

+ 1 - 1
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

+ 8 - 5
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;