@@ -59,13 +59,14 @@ namespace {
59
59
// angle = norm(a)
60
60
// axis = a.normalized()
61
61
// If norm(a) == 0, it returns an identity rotation.
62
- static inline Rotation RotationFromVector (const Vector3& a)
62
+ static inline void RotationFromVector (const Vector3& a, Rotation& r )
63
63
{
64
64
const double norm_a = Length (a);
65
65
if (norm_a < kEpsilon ) {
66
- return Rotation::Identity ();
66
+ r = Rotation::Identity ();
67
+ return ;
67
68
}
68
- return Rotation::FromAxisAndAngle (a / norm_a, norm_a);
69
+ r = Rotation::FromAxisAndAngle (a / norm_a, norm_a);
69
70
}
70
71
71
72
} // namespace
@@ -199,7 +200,8 @@ void SensorFusionEkf::ComputeMeasurementJacobian()
199
200
Vector3 delta = Vector3::Zero ();
200
201
delta[dof] = kFiniteDifferencingEpsilon ;
201
202
202
- const Rotation epsilon_rotation = RotationFromVector (delta);
203
+ Rotation epsilon_rotation;
204
+ RotationFromVector (delta, epsilon_rotation);
203
205
const Vector3 delta_rotation
204
206
= ComputeInnovation (epsilon_rotation * current_state_.sensor_from_start_rotation );
205
207
@@ -263,7 +265,8 @@ void SensorFusionEkf::ProcessAccelerometerSample(const AccelerometerData& sample
263
265
* state_covariance_;
264
266
265
267
// Updates pose and associate covariance matrix.
266
- const Rotation rotation_from_state_update = RotationFromVector (state_update_);
268
+ Rotation rotation_from_state_update;
269
+ RotationFromVector (state_update_, rotation_from_state_update);
267
270
268
271
current_state_.sensor_from_start_rotation
269
272
= rotation_from_state_update * current_state_.sensor_from_start_rotation ;
0 commit comments