1use nalgebra::{DMatrix, DVector, Matrix3, Matrix4, SMatrix, Vector3};
7use rust_robotics_core::{skew, so3_exp, so3_left_jacobian, so3_left_jacobian_inverse, so3_log};
8use rust_robotics_optimization::{
9 solve, Factor, FactorEvaluation, OptimizationError, OptimizationResult, Problem, SolverConfig,
10 SolverSummary, Variable, VariableId,
11};
12
13pub type Matrix9 = SMatrix<f64, 9, 9>;
14pub type Matrix9x6 = SMatrix<f64, 9, 6>;
15pub type Matrix6 = SMatrix<f64, 6, 6>;
16
17#[derive(Debug, Clone, Copy, PartialEq)]
19pub struct ImuMeasurement {
20 pub acceleration: Vector3<f64>,
21 pub angular_velocity: Vector3<f64>,
22}
23
24#[derive(Debug, Clone, Copy, PartialEq)]
26pub struct ImuExtrinsics {
27 pub rotation_body_from_sensor: Matrix3<f64>,
28 pub translation_body_from_sensor: Vector3<f64>,
29}
30
31impl ImuExtrinsics {
32 pub fn identity() -> Self {
33 Self {
34 rotation_body_from_sensor: Matrix3::identity(),
35 translation_body_from_sensor: Vector3::zeros(),
36 }
37 }
38
39 pub fn from_homogeneous(body_from_sensor: &Matrix4<f64>) -> OptimizationResult<Self> {
41 if !body_from_sensor.iter().all(|value| value.is_finite()) {
42 return Err(OptimizationError::InvalidParameter(
43 "IMU extrinsic transform must be finite".into(),
44 ));
45 }
46 let bottom_row = body_from_sensor.row(3);
47 if (bottom_row[0].abs() > 1.0e-12)
48 || (bottom_row[1].abs() > 1.0e-12)
49 || (bottom_row[2].abs() > 1.0e-12)
50 || ((bottom_row[3] - 1.0).abs() > 1.0e-12)
51 {
52 return Err(OptimizationError::InvalidParameter(
53 "IMU extrinsic transform must be homogeneous".into(),
54 ));
55 }
56 let rotation = body_from_sensor.fixed_view::<3, 3>(0, 0).into_owned();
57 let orthogonality_error = rotation.transpose() * rotation - Matrix3::identity();
58 if orthogonality_error.norm() > 1.0e-8 || (rotation.determinant() - 1.0).abs() > 1.0e-8 {
59 return Err(OptimizationError::InvalidParameter(
60 "IMU extrinsic rotation must be in SO(3)".into(),
61 ));
62 }
63 Ok(Self {
64 rotation_body_from_sensor: rotation,
65 translation_body_from_sensor: body_from_sensor.fixed_view::<3, 1>(0, 3).into_owned(),
66 })
67 }
68
69 pub fn transform(
74 &self,
75 measurement: ImuMeasurement,
76 angular_acceleration_sensor: Vector3<f64>,
77 ) -> ImuMeasurement {
78 let angular_velocity = self.rotation_body_from_sensor * measurement.angular_velocity;
79 let angular_acceleration = self.rotation_body_from_sensor * angular_acceleration_sensor;
80 let translation = self.translation_body_from_sensor;
81 let acceleration = self.rotation_body_from_sensor * measurement.acceleration
82 - skew(&angular_velocity) * skew(&angular_velocity) * translation
83 + skew(&translation) * angular_acceleration;
84 ImuMeasurement {
85 acceleration,
86 angular_velocity,
87 }
88 }
89}
90
91#[derive(Debug, Clone, Copy, PartialEq)]
93pub struct ImuBias {
94 pub accelerometer: Vector3<f64>,
95 pub gyroscope: Vector3<f64>,
96}
97
98impl ImuBias {
99 pub fn zero() -> Self {
100 Self {
101 accelerometer: Vector3::zeros(),
102 gyroscope: Vector3::zeros(),
103 }
104 }
105
106 fn vector(&self) -> DVector<f64> {
107 DVector::from_vec(vec![
108 self.accelerometer.x,
109 self.accelerometer.y,
110 self.accelerometer.z,
111 self.gyroscope.x,
112 self.gyroscope.y,
113 self.gyroscope.z,
114 ])
115 }
116}
117
118#[derive(Debug, Clone, PartialEq)]
120pub struct NavState {
121 pub rotation: Matrix3<f64>,
122 pub position: Vector3<f64>,
123 pub velocity: Vector3<f64>,
124}
125
126impl NavState {
127 pub fn identity() -> Self {
128 Self {
129 rotation: Matrix3::identity(),
130 position: Vector3::zeros(),
131 velocity: Vector3::zeros(),
132 }
133 }
134}
135
136#[derive(Debug, Clone, Copy, PartialEq)]
138pub struct ImuNoise {
139 pub accelerometer_sigma: f64,
140 pub gyroscope_sigma: f64,
141}
142
143impl Default for ImuNoise {
144 fn default() -> Self {
145 Self {
146 accelerometer_sigma: 0.1,
147 gyroscope_sigma: 0.01,
148 }
149 }
150}
151
152#[derive(Debug, Clone)]
154pub struct PreintegratedImuMeasurement {
155 pub delta_rotation: Matrix3<f64>,
156 pub delta_position: Vector3<f64>,
157 pub delta_velocity: Vector3<f64>,
158 pub delta_time: f64,
159 pub covariance: Matrix9,
160 pub bias_jacobian: Matrix9x6,
161 pub linearization_bias: ImuBias,
162 noise: ImuNoise,
163}
164
165impl PreintegratedImuMeasurement {
166 pub fn new(linearization_bias: ImuBias, noise: ImuNoise) -> Self {
167 Self {
168 delta_rotation: Matrix3::identity(),
169 delta_position: Vector3::zeros(),
170 delta_velocity: Vector3::zeros(),
171 delta_time: 0.0,
172 covariance: Matrix9::zeros(),
173 bias_jacobian: Matrix9x6::zeros(),
174 linearization_bias,
175 noise,
176 }
177 }
178
179 pub fn integrate(
181 &mut self,
182 acceleration: Vector3<f64>,
183 angular_velocity: Vector3<f64>,
184 dt: f64,
185 ) -> OptimizationResult<()> {
186 if !dt.is_finite() || dt <= 0.0 {
187 return Err(OptimizationError::InvalidParameter(
188 "IMU sample dt must be finite and positive".into(),
189 ));
190 }
191 let acceleration = acceleration - self.linearization_bias.accelerometer;
192 let angular_velocity = angular_velocity - self.linearization_bias.gyroscope;
193 let rotation = self.delta_rotation;
194 let rotated_acceleration = rotation * acceleration;
195 let half_dt_squared = 0.5 * dt * dt;
196
197 self.delta_position += self.delta_velocity * dt + rotated_acceleration * half_dt_squared;
198 self.delta_velocity += rotated_acceleration * dt;
199 self.delta_rotation *= so3_exp(&(angular_velocity * dt));
200 self.delta_time += dt;
201
202 let mut transition = Matrix9::identity();
203 transition
204 .fixed_view_mut::<3, 3>(0, 0)
205 .copy_from(&so3_exp(&(-angular_velocity * dt)));
206 transition
207 .fixed_view_mut::<3, 3>(3, 0)
208 .copy_from(&(-rotation * skew(&acceleration) * half_dt_squared));
209 transition
210 .fixed_view_mut::<3, 3>(3, 6)
211 .copy_from(&(Matrix3::identity() * dt));
212 transition
213 .fixed_view_mut::<3, 3>(6, 0)
214 .copy_from(&(-rotation * skew(&acceleration) * dt));
215
216 let mut noise_jacobian = Matrix9x6::zeros();
217 noise_jacobian
218 .fixed_view_mut::<3, 3>(0, 3)
219 .copy_from(&(Matrix3::identity() * dt));
220 noise_jacobian
221 .fixed_view_mut::<3, 3>(3, 0)
222 .copy_from(&(rotation * half_dt_squared));
223 noise_jacobian
224 .fixed_view_mut::<3, 3>(6, 0)
225 .copy_from(&(rotation * dt));
226
227 let mut measurement_covariance = Matrix6::zeros();
228 measurement_covariance
229 .fixed_view_mut::<3, 3>(0, 0)
230 .copy_from(&(Matrix3::identity() * self.noise.accelerometer_sigma.powi(2)));
231 measurement_covariance
232 .fixed_view_mut::<3, 3>(3, 3)
233 .copy_from(&(Matrix3::identity() * self.noise.gyroscope_sigma.powi(2)));
234
235 self.covariance = transition * self.covariance * transition.transpose()
236 + noise_jacobian * measurement_covariance * noise_jacobian.transpose();
237 self.bias_jacobian = transition * self.bias_jacobian - noise_jacobian;
238 Ok(())
239 }
240
241 pub fn integrate_sensor_measurement(
243 &mut self,
244 measurement: ImuMeasurement,
245 angular_acceleration_sensor: Vector3<f64>,
246 extrinsics: &ImuExtrinsics,
247 dt: f64,
248 ) -> OptimizationResult<()> {
249 let body_measurement = extrinsics.transform(measurement, angular_acceleration_sensor);
250 self.integrate(
251 body_measurement.acceleration,
252 body_measurement.angular_velocity,
253 dt,
254 )
255 }
256
257 pub fn predict(&self, state: &NavState, bias: &ImuBias, gravity: Vector3<f64>) -> NavState {
259 let (corrected_rotation, corrected_position, corrected_velocity) =
260 self.corrected_delta(bias);
261 let dt = self.delta_time;
262
263 NavState {
264 rotation: state.rotation * corrected_rotation,
265 position: state.position
266 + state.velocity * dt
267 + gravity * (0.5 * dt * dt)
268 + state.rotation * corrected_position,
269 velocity: state.velocity + gravity * dt + state.rotation * corrected_velocity,
270 }
271 }
272
273 fn corrected_delta(&self, bias: &ImuBias) -> (Matrix3<f64>, Vector3<f64>, Vector3<f64>) {
274 let bias_delta = bias.vector() - self.linearization_bias.vector();
275 let correction = self.bias_jacobian * bias_delta;
276 (
277 self.delta_rotation * so3_exp(&Vector3::from(correction.fixed_rows::<3>(0))),
278 self.delta_position + Vector3::from(correction.fixed_rows::<3>(3)),
279 self.delta_velocity + Vector3::from(correction.fixed_rows::<3>(6)),
280 )
281 }
282}
283
284#[derive(Debug, Clone, PartialEq)]
286pub struct NavStateDelta {
287 pub rotation: Matrix3<f64>,
288 pub position: Vector3<f64>,
289 pub velocity: Vector3<f64>,
290}
291
292impl NavStateDelta {
293 pub fn identity() -> Self {
294 Self {
295 rotation: Matrix3::identity(),
296 position: Vector3::zeros(),
297 velocity: Vector3::zeros(),
298 }
299 }
300}
301
302pub fn nav_state_variable(state: &NavState) -> Variable {
304 Variable::with_retraction(encode_nav_state(state), 9, retract_nav_state)
305 .expect("navigation state dimensions are valid")
306}
307
308pub fn imu_bias_variable(bias: &ImuBias) -> Variable {
310 Variable::euclidean(bias.vector())
311}
312
313pub struct BiasPriorFactor {
315 variables: [VariableId; 1],
316 measurement: ImuBias,
317 information: Matrix6,
318}
319
320impl BiasPriorFactor {
321 pub fn new(variable: VariableId, measurement: ImuBias, information: Matrix6) -> Self {
322 Self {
323 variables: [variable],
324 measurement,
325 information,
326 }
327 }
328}
329
330impl Factor for BiasPriorFactor {
331 fn variable_ids(&self) -> &[VariableId] {
332 &self.variables
333 }
334
335 fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
336 expect_dimensions(values, &[6], "bias prior")?;
337 Ok(FactorEvaluation {
338 residual: values[0] - self.measurement.vector(),
339 information: fixed_to_dynamic(&self.information),
340 jacobians: vec![DMatrix::identity(6, 6)],
341 })
342 }
343}
344
345pub struct BiasBetweenFactor {
347 variables: [VariableId; 2],
348 information: Matrix6,
349}
350
351impl BiasBetweenFactor {
352 pub fn new(bias_i: VariableId, bias_j: VariableId, information: Matrix6) -> Self {
353 Self {
354 variables: [bias_i, bias_j],
355 information,
356 }
357 }
358}
359
360impl Factor for BiasBetweenFactor {
361 fn variable_ids(&self) -> &[VariableId] {
362 &self.variables
363 }
364
365 fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
366 expect_dimensions(values, &[6, 6], "bias between")?;
367 Ok(FactorEvaluation {
368 residual: values[0] - values[1],
369 information: fixed_to_dynamic(&self.information),
370 jacobians: vec![DMatrix::identity(6, 6), -DMatrix::identity(6, 6)],
371 })
372 }
373}
374
375pub struct NavStatePriorFactor {
377 variables: [VariableId; 1],
378 measurement: NavState,
379 information: Matrix9,
380}
381
382impl NavStatePriorFactor {
383 pub fn new(variable: VariableId, measurement: NavState, information: Matrix9) -> Self {
384 Self {
385 variables: [variable],
386 measurement,
387 information,
388 }
389 }
390
391 fn residual(&self, value: &DVector<f64>) -> nalgebra::SVector<f64, 9> {
392 let state = decode_nav_state(value);
393 nav_state_local(&state, &self.measurement)
394 }
395}
396
397impl Factor for NavStatePriorFactor {
398 fn variable_ids(&self) -> &[VariableId] {
399 &self.variables
400 }
401
402 fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
403 expect_dimensions(values, &[9], "navigation-state prior")?;
404 let state = decode_nav_state(values[0]);
405 let residual = self.residual(values[0]);
406 let rotation_residual = Vector3::from(residual.fixed_rows::<3>(0));
407 let local_position = Vector3::from(residual.fixed_rows::<3>(3));
408 let local_velocity = Vector3::from(residual.fixed_rows::<3>(6));
409 let inverse_rotation = state.rotation.transpose();
410 let mut jacobian = DMatrix::zeros(9, 9);
411 jacobian
412 .view_mut((0, 0), (3, 3))
413 .copy_from(&(-so3_left_jacobian_inverse(&rotation_residual)));
414 jacobian
415 .view_mut((3, 0), (3, 3))
416 .copy_from(&skew(&local_position));
417 jacobian
418 .view_mut((3, 3), (3, 3))
419 .copy_from(&(-inverse_rotation));
420 jacobian
421 .view_mut((6, 0), (3, 3))
422 .copy_from(&skew(&local_velocity));
423 jacobian
424 .view_mut((6, 6), (3, 3))
425 .copy_from(&(-inverse_rotation));
426 Ok(FactorEvaluation {
427 residual: DVector::from_column_slice(residual.as_slice()),
428 information: fixed_to_dynamic(&self.information),
429 jacobians: vec![jacobian],
430 })
431 }
432}
433
434pub struct PositionVelocityFactor {
436 variables: [VariableId; 2],
437 delta_time: f64,
438 information: Matrix3<f64>,
439}
440
441impl PositionVelocityFactor {
442 pub fn new(
443 state_i: VariableId,
444 state_j: VariableId,
445 delta_time: f64,
446 information: Matrix3<f64>,
447 ) -> OptimizationResult<Self> {
448 if !delta_time.is_finite() || delta_time <= 0.0 {
449 return Err(OptimizationError::InvalidParameter(
450 "position/velocity factor dt must be finite and positive".into(),
451 ));
452 }
453 Ok(Self {
454 variables: [state_i, state_j],
455 delta_time,
456 information,
457 })
458 }
459}
460
461impl Factor for PositionVelocityFactor {
462 fn variable_ids(&self) -> &[VariableId] {
463 &self.variables
464 }
465
466 fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
467 expect_dimensions(values, &[9, 9], "position/velocity")?;
468 let state_i = decode_nav_state(values[0]);
469 let state_j = decode_nav_state(values[1]);
470 let inverse_dt = self.delta_time.recip();
471 let residual = state_i.velocity - (state_j.position - state_i.position) * inverse_dt;
472 let mut jacobian_i = DMatrix::zeros(3, 9);
473 jacobian_i
474 .view_mut((0, 3), (3, 3))
475 .copy_from(&(Matrix3::identity() * inverse_dt));
476 jacobian_i
477 .view_mut((0, 6), (3, 3))
478 .copy_from(&Matrix3::identity());
479 let mut jacobian_j = DMatrix::zeros(3, 9);
480 jacobian_j
481 .view_mut((0, 3), (3, 3))
482 .copy_from(&(-Matrix3::identity() * inverse_dt));
483 Ok(FactorEvaluation {
484 residual: DVector::from_column_slice(residual.as_slice()),
485 information: fixed_to_dynamic(&self.information),
486 jacobians: vec![jacobian_i, jacobian_j],
487 })
488 }
489}
490
491pub struct NavStateBetweenFactor {
493 variables: [VariableId; 2],
494 measurement: NavStateDelta,
495 information: Matrix9,
496}
497
498impl NavStateBetweenFactor {
499 pub fn new(
500 state_i: VariableId,
501 state_j: VariableId,
502 measurement: NavStateDelta,
503 information: Matrix9,
504 ) -> Self {
505 Self {
506 variables: [state_i, state_j],
507 measurement,
508 information,
509 }
510 }
511
512 fn residual(
513 &self,
514 value_i: &DVector<f64>,
515 value_j: &DVector<f64>,
516 ) -> nalgebra::SVector<f64, 9> {
517 let state_i = decode_nav_state(value_i);
518 let state_j = decode_nav_state(value_j);
519 let predicted = nav_state_delta(&state_i, &state_j);
520 let rotation_error = so3_log(&(self.measurement.rotation.transpose() * predicted.rotation));
521 nalgebra::SVector::<f64, 9>::from_iterator(
522 rotation_error
523 .iter()
524 .chain((predicted.position - self.measurement.position).iter())
525 .chain((predicted.velocity - self.measurement.velocity).iter())
526 .copied(),
527 )
528 }
529}
530
531impl Factor for NavStateBetweenFactor {
532 fn variable_ids(&self) -> &[VariableId] {
533 &self.variables
534 }
535
536 fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
537 expect_dimensions(values, &[9, 9], "navigation-state between")?;
538 let state_i = decode_nav_state(values[0]);
539 let state_j = decode_nav_state(values[1]);
540 let residual = self.residual(values[0], values[1]);
541 let local_position = state_i.rotation.transpose() * (state_j.position - state_i.position);
542 let local_velocity = state_i.rotation.transpose() * (state_j.velocity - state_i.velocity);
543 let rotation_residual = Vector3::from(residual.fixed_rows::<3>(0));
544 let inverse_rotation_i = state_i.rotation.transpose();
545 let mut jacobian_i = DMatrix::zeros(9, 9);
546 jacobian_i.view_mut((0, 0), (3, 3)).copy_from(
547 &(-so3_left_jacobian_inverse(&rotation_residual)
548 * self.measurement.rotation.transpose()),
549 );
550 jacobian_i
551 .view_mut((3, 0), (3, 3))
552 .copy_from(&skew(&local_position));
553 jacobian_i
554 .view_mut((3, 3), (3, 3))
555 .copy_from(&(-inverse_rotation_i));
556 jacobian_i
557 .view_mut((6, 0), (3, 3))
558 .copy_from(&skew(&local_velocity));
559 jacobian_i
560 .view_mut((6, 6), (3, 3))
561 .copy_from(&(-inverse_rotation_i));
562
563 let mut jacobian_j = DMatrix::zeros(9, 9);
564 jacobian_j
565 .view_mut((0, 0), (3, 3))
566 .copy_from(&so3_left_jacobian_inverse(&(-rotation_residual)));
567 jacobian_j
568 .view_mut((3, 3), (3, 3))
569 .copy_from(&inverse_rotation_i);
570 jacobian_j
571 .view_mut((6, 6), (3, 3))
572 .copy_from(&inverse_rotation_i);
573 Ok(FactorEvaluation {
574 residual: DVector::from_column_slice(residual.as_slice()),
575 information: fixed_to_dynamic(&self.information),
576 jacobians: vec![jacobian_i, jacobian_j],
577 })
578 }
579}
580
581pub struct ImuFactor {
583 variables: [VariableId; 3],
584 measurement: PreintegratedImuMeasurement,
585 gravity: Vector3<f64>,
586}
587
588impl ImuFactor {
589 pub fn new(
590 state_i: VariableId,
591 state_j: VariableId,
592 bias_i: VariableId,
593 measurement: PreintegratedImuMeasurement,
594 gravity: Vector3<f64>,
595 ) -> Self {
596 Self {
597 variables: [state_i, state_j, bias_i],
598 measurement,
599 gravity,
600 }
601 }
602}
603
604impl Factor for ImuFactor {
605 fn variable_ids(&self) -> &[VariableId] {
606 &self.variables
607 }
608
609 fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
610 if values[0].len() != 9 || values[1].len() != 9 || values[2].len() != 6 {
611 return Err(OptimizationError::InvalidFactor(
612 "IMU factor expects state(9), state(9), bias(6)".into(),
613 ));
614 }
615 let residual = self.residual(values);
616 let jacobians = self.analytic_jacobians(values, &residual);
617 let regularized = self.measurement.covariance + Matrix9::identity() * 1.0e-12;
618 let information = regularized
619 .try_inverse()
620 .ok_or(OptimizationError::LinearSolveFailed)?;
621 Ok(FactorEvaluation {
622 residual: DVector::from_column_slice(residual.as_slice()),
623 information: DMatrix::from_column_slice(9, 9, information.as_slice()),
624 jacobians,
625 })
626 }
627}
628
629impl ImuFactor {
630 fn residual(&self, values: &[&DVector<f64>]) -> nalgebra::SVector<f64, 9> {
631 let state_i = decode_nav_state(values[0]);
632 let state_j = decode_nav_state(values[1]);
633 let bias = decode_bias(values[2]);
634 let (delta_rotation, delta_position, delta_velocity) =
635 self.measurement.corrected_delta(&bias);
636 let dt = self.measurement.delta_time;
637 let rotation_error = so3_log(
638 &(delta_rotation.transpose() * state_i.rotation.transpose() * state_j.rotation),
639 );
640 let position_error = state_i.rotation.transpose()
641 * (state_j.position
642 - state_i.position
643 - state_i.velocity * dt
644 - self.gravity * (0.5 * dt * dt))
645 - delta_position;
646 let velocity_error = state_i.rotation.transpose()
647 * (state_j.velocity - state_i.velocity - self.gravity * dt)
648 - delta_velocity;
649 nalgebra::SVector::<f64, 9>::from_iterator(
650 rotation_error
651 .iter()
652 .chain(position_error.iter())
653 .chain(velocity_error.iter())
654 .copied(),
655 )
656 }
657
658 fn analytic_jacobians(
659 &self,
660 values: &[&DVector<f64>],
661 residual: &nalgebra::SVector<f64, 9>,
662 ) -> Vec<DMatrix<f64>> {
663 let state_i = decode_nav_state(values[0]);
664 let state_j = decode_nav_state(values[1]);
665 let bias = decode_bias(values[2]);
666 let inverse_rotation_i = state_i.rotation.transpose();
667 let dt = self.measurement.delta_time;
668 let position_delta = state_j.position
669 - state_i.position
670 - state_i.velocity * dt
671 - self.gravity * (0.5 * dt * dt);
672 let velocity_delta = state_j.velocity - state_i.velocity - self.gravity * dt;
673 let local_position = inverse_rotation_i * position_delta;
674 let local_velocity = inverse_rotation_i * velocity_delta;
675 let rotation_residual = Vector3::from(residual.fixed_rows::<3>(0));
676 let left_inverse = so3_left_jacobian_inverse(&rotation_residual);
677 let right_inverse = so3_left_jacobian_inverse(&(-rotation_residual));
678 let (corrected_rotation, _, _) = self.measurement.corrected_delta(&bias);
679
680 let mut state_i_jacobian = DMatrix::zeros(9, 9);
681 state_i_jacobian
682 .view_mut((0, 0), (3, 3))
683 .copy_from(&(-left_inverse * corrected_rotation.transpose()));
684 state_i_jacobian
685 .view_mut((3, 0), (3, 3))
686 .copy_from(&skew(&local_position));
687 state_i_jacobian
688 .view_mut((3, 3), (3, 3))
689 .copy_from(&(-inverse_rotation_i));
690 state_i_jacobian
691 .view_mut((3, 6), (3, 3))
692 .copy_from(&(-inverse_rotation_i * dt));
693 state_i_jacobian
694 .view_mut((6, 0), (3, 3))
695 .copy_from(&skew(&local_velocity));
696 state_i_jacobian
697 .view_mut((6, 6), (3, 3))
698 .copy_from(&(-inverse_rotation_i));
699
700 let mut state_j_jacobian = DMatrix::zeros(9, 9);
701 state_j_jacobian
702 .view_mut((0, 0), (3, 3))
703 .copy_from(&right_inverse);
704 state_j_jacobian
705 .view_mut((3, 3), (3, 3))
706 .copy_from(&inverse_rotation_i);
707 state_j_jacobian
708 .view_mut((6, 6), (3, 3))
709 .copy_from(&inverse_rotation_i);
710
711 let bias_delta = bias.vector() - self.measurement.linearization_bias.vector();
712 let correction = self.measurement.bias_jacobian * bias_delta;
713 let rotation_correction = Vector3::from(correction.fixed_rows::<3>(0));
714 let correction_right_jacobian = so3_left_jacobian(&(-rotation_correction));
715 let mut bias_jacobian = DMatrix::zeros(9, 6);
716 bias_jacobian.view_mut((0, 0), (3, 6)).copy_from(
717 &(-left_inverse
718 * correction_right_jacobian
719 * self.measurement.bias_jacobian.fixed_rows::<3>(0)),
720 );
721 bias_jacobian
722 .view_mut((3, 0), (3, 6))
723 .copy_from(&(-self.measurement.bias_jacobian.fixed_rows::<3>(3)));
724 bias_jacobian
725 .view_mut((6, 0), (3, 6))
726 .copy_from(&(-self.measurement.bias_jacobian.fixed_rows::<3>(6)));
727
728 vec![state_i_jacobian, state_j_jacobian, bias_jacobian]
729 }
730
731 #[cfg(test)]
732 fn numerical_jacobian(&self, values: &[&DVector<f64>], variable: usize) -> DMatrix<f64> {
733 let dimension = if variable == 2 { 6 } else { 9 };
734 let epsilon = 1.0e-6;
735 let mut jacobian = DMatrix::zeros(9, dimension);
736 for column in 0..dimension {
737 let mut increment = DVector::zeros(dimension);
738 increment[column] = epsilon;
739 let plus_value = if variable == 2 {
740 values[variable] + &increment
741 } else {
742 retract_nav_state(values[variable], &increment).expect("dimensions are valid")
743 };
744 let minus_value = if variable == 2 {
745 values[variable] - &increment
746 } else {
747 retract_nav_state(values[variable], &(-&increment)).expect("dimensions are valid")
748 };
749 let mut plus_values = values.to_vec();
750 let mut minus_values = values.to_vec();
751 plus_values[variable] = &plus_value;
752 minus_values[variable] = &minus_value;
753 let derivative =
754 (self.residual(&plus_values) - self.residual(&minus_values)) / (2.0 * epsilon);
755 jacobian.set_column(column, &derivative);
756 }
757 jacobian
758 }
759}
760
761#[derive(Debug, Clone)]
763pub struct ImuTrajectoryConfig {
764 pub solver: SolverConfig,
765 pub nav_prior_information: Matrix9,
766 pub nav_measurement_information: Option<Matrix9>,
771 pub bias_prior_information: Matrix6,
772 pub bias_between_information: Matrix6,
773}
774
775impl Default for ImuTrajectoryConfig {
776 fn default() -> Self {
777 Self {
778 solver: SolverConfig::default(),
779 nav_prior_information: Matrix9::identity() * 1.0e8,
780 nav_measurement_information: None,
781 bias_prior_information: Matrix6::identity() * 1.0e4,
782 bias_between_information: Matrix6::identity() * 1.0e3,
783 }
784 }
785}
786
787#[derive(Debug, Clone)]
789pub struct ImuTrajectoryResult {
790 pub states: Vec<NavState>,
791 pub biases: Vec<ImuBias>,
792 pub summary: SolverSummary,
793}
794
795pub fn optimize_imu_trajectory(
800 initial_states: &[NavState],
801 initial_bias: ImuBias,
802 measurements: &[PreintegratedImuMeasurement],
803 gravity: Vector3<f64>,
804 config: &ImuTrajectoryConfig,
805) -> OptimizationResult<ImuTrajectoryResult> {
806 if initial_states.len() < 2 || measurements.len() + 1 != initial_states.len() {
807 return Err(OptimizationError::InvalidParameter(
808 "IMU trajectory requires N states and N-1 measurements".into(),
809 ));
810 }
811 let mut problem = Problem::new();
812 let state_ids = initial_states
813 .iter()
814 .map(|state| problem.add_variable(nav_state_variable(state)))
815 .collect::<Vec<_>>();
816 let bias_ids = initial_states
817 .iter()
818 .map(|_| problem.add_variable(imu_bias_variable(&initial_bias)))
819 .collect::<Vec<_>>();
820
821 problem.add_factor(NavStatePriorFactor::new(
822 state_ids[0],
823 initial_states[0].clone(),
824 config.nav_prior_information,
825 ))?;
826 problem.add_factor(BiasPriorFactor::new(
827 bias_ids[0],
828 initial_bias,
829 config.bias_prior_information,
830 ))?;
831 if let Some(information) = config.nav_measurement_information {
832 for (id, measurement) in state_ids.iter().zip(initial_states) {
833 problem.add_factor(NavStatePriorFactor::new(
834 *id,
835 measurement.clone(),
836 information,
837 ))?;
838 }
839 }
840 for (index, measurement) in measurements.iter().enumerate() {
841 problem.add_factor(ImuFactor::new(
842 state_ids[index],
843 state_ids[index + 1],
844 bias_ids[index],
845 measurement.clone(),
846 gravity,
847 ))?;
848 problem.add_factor(BiasBetweenFactor::new(
849 bias_ids[index],
850 bias_ids[index + 1],
851 config.bias_between_information,
852 ))?;
853 }
854
855 let summary = solve(&mut problem, &config.solver)?;
856 let states = state_ids
857 .iter()
858 .map(|id| {
859 problem
860 .variable(*id)
861 .map(|variable| decode_nav_state(variable.value()))
862 })
863 .collect::<OptimizationResult<Vec<_>>>()?;
864 let biases = bias_ids
865 .iter()
866 .map(|id| {
867 problem
868 .variable(*id)
869 .map(|variable| decode_bias(variable.value()))
870 })
871 .collect::<OptimizationResult<Vec<_>>>()?;
872 Ok(ImuTrajectoryResult {
873 states,
874 biases,
875 summary,
876 })
877}
878
879fn nav_state_delta(from: &NavState, to: &NavState) -> NavStateDelta {
880 let inverse_rotation = from.rotation.transpose();
881 NavStateDelta {
882 rotation: inverse_rotation * to.rotation,
883 position: inverse_rotation * (to.position - from.position),
884 velocity: inverse_rotation * (to.velocity - from.velocity),
885 }
886}
887
888fn nav_state_local(from: &NavState, to: &NavState) -> nalgebra::SVector<f64, 9> {
889 let delta = nav_state_delta(from, to);
890 let rotation = so3_log(&delta.rotation);
891 nalgebra::SVector::<f64, 9>::from_iterator(
892 rotation
893 .iter()
894 .chain(delta.position.iter())
895 .chain(delta.velocity.iter())
896 .copied(),
897 )
898}
899
900fn expect_dimensions(
901 values: &[&DVector<f64>],
902 dimensions: &[usize],
903 factor_name: &str,
904) -> OptimizationResult<()> {
905 if values.len() != dimensions.len()
906 || values
907 .iter()
908 .zip(dimensions)
909 .any(|(value, dimension)| value.len() != *dimension)
910 {
911 return Err(OptimizationError::InvalidFactor(format!(
912 "{factor_name} factor received invalid variable dimensions"
913 )));
914 }
915 Ok(())
916}
917
918fn fixed_to_dynamic<const N: usize>(matrix: &SMatrix<f64, N, N>) -> DMatrix<f64> {
919 DMatrix::from_column_slice(N, N, matrix.as_slice())
920}
921
922fn encode_nav_state(state: &NavState) -> DVector<f64> {
923 let rotation = so3_log(&state.rotation);
924 DVector::from_vec(vec![
925 rotation.x,
926 rotation.y,
927 rotation.z,
928 state.position.x,
929 state.position.y,
930 state.position.z,
931 state.velocity.x,
932 state.velocity.y,
933 state.velocity.z,
934 ])
935}
936
937fn decode_nav_state(value: &DVector<f64>) -> NavState {
938 NavState {
939 rotation: so3_exp(&Vector3::new(value[0], value[1], value[2])),
940 position: Vector3::new(value[3], value[4], value[5]),
941 velocity: Vector3::new(value[6], value[7], value[8]),
942 }
943}
944
945fn decode_bias(value: &DVector<f64>) -> ImuBias {
946 ImuBias {
947 accelerometer: Vector3::new(value[0], value[1], value[2]),
948 gyroscope: Vector3::new(value[3], value[4], value[5]),
949 }
950}
951
952fn retract_nav_state(
953 value: &DVector<f64>,
954 increment: &DVector<f64>,
955) -> OptimizationResult<DVector<f64>> {
956 if value.len() != 9 || increment.len() != 9 {
957 return Err(OptimizationError::InvalidParameter(
958 "navigation state and increment must have dimension 9".into(),
959 ));
960 }
961 let state = decode_nav_state(value);
962 let updated = NavState {
963 rotation: state.rotation * so3_exp(&Vector3::new(increment[0], increment[1], increment[2])),
964 position: state.position + Vector3::new(increment[3], increment[4], increment[5]),
965 velocity: state.velocity + Vector3::new(increment[6], increment[7], increment[8]),
966 };
967 Ok(encode_nav_state(&updated))
968}
969
970#[cfg(test)]
971mod tests {
972 use rust_robotics_optimization::{Factor, Problem};
973
974 use super::*;
975
976 fn numerical_factor_jacobian(
977 factor: &dyn Factor,
978 values: &[DVector<f64>],
979 variable: usize,
980 nav_state: bool,
981 ) -> DMatrix<f64> {
982 let residual_dimension = {
983 let refs = values.iter().collect::<Vec<_>>();
984 factor.evaluate(&refs).unwrap().residual.len()
985 };
986 let dimension = values[variable].len();
987 let epsilon = 1.0e-6;
988 let mut jacobian = DMatrix::zeros(residual_dimension, dimension);
989 for column in 0..dimension {
990 let mut increment = DVector::zeros(dimension);
991 increment[column] = epsilon;
992 let mut plus = values.to_vec();
993 let mut minus = values.to_vec();
994 plus[variable] = if nav_state {
995 retract_nav_state(&values[variable], &increment).unwrap()
996 } else {
997 &values[variable] + &increment
998 };
999 minus[variable] = if nav_state {
1000 retract_nav_state(&values[variable], &(-&increment)).unwrap()
1001 } else {
1002 &values[variable] - &increment
1003 };
1004 let plus_refs = plus.iter().collect::<Vec<_>>();
1005 let minus_refs = minus.iter().collect::<Vec<_>>();
1006 let derivative = (factor.evaluate(&plus_refs).unwrap().residual
1007 - factor.evaluate(&minus_refs).unwrap().residual)
1008 / (2.0 * epsilon);
1009 jacobian.set_column(column, &derivative);
1010 }
1011 jacobian
1012 }
1013
1014 #[test]
1015 fn imu_extrinsics_include_rotation_and_lever_arm_terms() {
1016 let extrinsics = ImuExtrinsics {
1017 rotation_body_from_sensor: so3_exp(&Vector3::new(0.1, -0.2, 0.3)),
1018 translation_body_from_sensor: Vector3::new(0.4, -0.2, 0.1),
1019 };
1020 let measurement = ImuMeasurement {
1021 acceleration: Vector3::new(0.3, -0.1, 9.7),
1022 angular_velocity: Vector3::new(0.2, -0.3, 0.4),
1023 };
1024 let angular_acceleration = Vector3::new(0.05, 0.02, -0.04);
1025 let transformed = extrinsics.transform(measurement, angular_acceleration);
1026 let rotation = extrinsics.rotation_body_from_sensor;
1027 let omega = rotation * measurement.angular_velocity;
1028 let alpha = rotation * angular_acceleration;
1029 let expected_acceleration = rotation * measurement.acceleration
1030 - skew(&omega) * skew(&omega) * extrinsics.translation_body_from_sensor
1031 + skew(&extrinsics.translation_body_from_sensor) * alpha;
1032 assert!((transformed.acceleration - expected_acceleration).norm() < 1.0e-12);
1033 assert!((transformed.angular_velocity - omega).norm() < 1.0e-12);
1034
1035 let identity = ImuExtrinsics::identity().transform(measurement, angular_acceleration);
1036 assert_eq!(identity, measurement);
1037 }
1038
1039 #[test]
1040 fn specialized_factor_jacobians_match_finite_differences() {
1041 let state_i = NavState {
1042 rotation: so3_exp(&Vector3::new(0.2, -0.1, 0.3)),
1043 position: Vector3::new(1.0, -2.0, 0.5),
1044 velocity: Vector3::new(0.4, 0.1, -0.2),
1045 };
1046 let state_j = NavState {
1047 rotation: so3_exp(&Vector3::new(-0.1, 0.3, 0.2)),
1048 position: Vector3::new(1.3, -1.8, 0.7),
1049 velocity: Vector3::new(0.2, 0.5, -0.1),
1050 };
1051 let measurement = NavState {
1052 rotation: so3_exp(&Vector3::new(0.25, -0.05, 0.28)),
1053 position: Vector3::new(0.9, -1.9, 0.55),
1054 velocity: Vector3::new(0.35, 0.2, -0.15),
1055 };
1056 let value_i = encode_nav_state(&state_i);
1057 let value_j = encode_nav_state(&state_j);
1058
1059 let prior = NavStatePriorFactor::new(VariableId(0), measurement, Matrix9::identity());
1060 let prior_values = vec![value_i.clone()];
1061 let prior_refs = prior_values.iter().collect::<Vec<_>>();
1062 let prior_evaluation = prior.evaluate(&prior_refs).unwrap();
1063 let prior_numerical = numerical_factor_jacobian(&prior, &prior_values, 0, true);
1064 assert!(
1065 (&prior_evaluation.jacobians[0] - prior_numerical).norm() < 1.0e-5,
1066 "navigation prior analytic Jacobian disagrees with finite differences"
1067 );
1068
1069 let between_measurement = NavStateDelta {
1070 rotation: so3_exp(&Vector3::new(-0.2, 0.1, 0.15)),
1071 position: Vector3::new(0.2, 0.1, -0.05),
1072 velocity: Vector3::new(-0.1, 0.25, 0.08),
1073 };
1074 let between = NavStateBetweenFactor::new(
1075 VariableId(0),
1076 VariableId(1),
1077 between_measurement,
1078 Matrix9::identity(),
1079 );
1080 let between_values = vec![value_i.clone(), value_j.clone()];
1081 let between_refs = between_values.iter().collect::<Vec<_>>();
1082 let between_evaluation = between.evaluate(&between_refs).unwrap();
1083 for variable in 0..2 {
1084 let numerical = numerical_factor_jacobian(&between, &between_values, variable, true);
1085 assert!(
1086 (&between_evaluation.jacobians[variable] - numerical).norm() < 1.0e-5,
1087 "navigation between analytic Jacobian {variable} disagrees with finite differences"
1088 );
1089 }
1090
1091 let position_velocity =
1092 PositionVelocityFactor::new(VariableId(0), VariableId(1), 0.2, Matrix3::identity())
1093 .unwrap();
1094 let pv_evaluation = position_velocity.evaluate(&between_refs).unwrap();
1095 for variable in 0..2 {
1096 let numerical =
1097 numerical_factor_jacobian(&position_velocity, &between_values, variable, true);
1098 assert!(
1099 (&pv_evaluation.jacobians[variable] - numerical).norm() < 1.0e-7,
1100 "position/velocity analytic Jacobian {variable} disagrees with finite differences"
1101 );
1102 }
1103 }
1104
1105 #[test]
1106 fn bias_factors_match_mathematical_robotics_residuals() {
1107 let bias_i = ImuBias {
1108 accelerometer: Vector3::new(0.1, -0.2, 0.3),
1109 gyroscope: Vector3::new(-0.01, 0.02, 0.03),
1110 };
1111 let bias_j = ImuBias {
1112 accelerometer: Vector3::new(0.08, -0.18, 0.25),
1113 gyroscope: Vector3::new(-0.02, 0.01, 0.04),
1114 };
1115 let prior = BiasPriorFactor::new(VariableId(0), bias_j, Matrix6::identity());
1116 let prior_value = bias_i.vector();
1117 let prior_evaluation = prior.evaluate(&[&prior_value]).unwrap();
1118 assert!((prior_evaluation.residual - (bias_i.vector() - bias_j.vector())).norm() < 1.0e-12);
1119
1120 let between = BiasBetweenFactor::new(VariableId(0), VariableId(1), Matrix6::identity());
1121 let value_i = bias_i.vector();
1122 let value_j = bias_j.vector();
1123 let evaluation = between.evaluate(&[&value_i, &value_j]).unwrap();
1124 assert!((evaluation.residual - (bias_i.vector() - bias_j.vector())).norm() < 1.0e-12);
1125 let values = vec![value_i, value_j];
1126 for variable in 0..2 {
1127 let numerical = numerical_factor_jacobian(&between, &values, variable, false);
1128 assert!(
1129 (&evaluation.jacobians[variable] - numerical).norm() < 1.0e-9,
1130 "bias-between analytic Jacobian {variable} disagrees with finite differences"
1131 );
1132 }
1133 }
1134
1135 #[test]
1136 fn stationary_imu_prediction_cancels_gravity() {
1137 let mut measurement =
1138 PreintegratedImuMeasurement::new(ImuBias::zero(), ImuNoise::default());
1139 for _ in 0..100 {
1140 measurement
1141 .integrate(Vector3::new(0.0, 0.0, 9.81), Vector3::zeros(), 0.01)
1142 .unwrap();
1143 }
1144 let predicted = measurement.predict(
1145 &NavState::identity(),
1146 &ImuBias::zero(),
1147 Vector3::new(0.0, 0.0, -9.81),
1148 );
1149 assert!(predicted.position.norm() < 1.0e-9);
1150 assert!(predicted.velocity.norm() < 1.0e-9);
1151 assert!(measurement.covariance.trace() > 0.0);
1152 }
1153
1154 #[test]
1155 fn imu_factor_has_zero_residual_for_prediction() {
1156 let mut measurement =
1157 PreintegratedImuMeasurement::new(ImuBias::zero(), ImuNoise::default());
1158 measurement
1159 .integrate(
1160 Vector3::new(0.2, -0.1, 9.81),
1161 Vector3::new(0.01, 0.02, -0.01),
1162 0.1,
1163 )
1164 .unwrap();
1165 let state_i = NavState {
1166 rotation: so3_exp(&Vector3::new(0.2, -0.1, 0.3)),
1167 position: Vector3::new(1.0, -2.0, 0.5),
1168 velocity: Vector3::new(0.4, 0.1, -0.2),
1169 };
1170 let gravity = Vector3::new(0.0, 0.0, -9.81);
1171 let state_j = measurement.predict(&state_i, &ImuBias::zero(), gravity);
1172 let mut problem = Problem::new();
1173 let id_i = problem.add_variable(nav_state_variable(&state_i));
1174 let id_j = problem.add_variable(nav_state_variable(&state_j));
1175 let bias_id = problem.add_variable(imu_bias_variable(&ImuBias::zero()));
1176 let factor = ImuFactor::new(id_i, id_j, bias_id, measurement, gravity);
1177 let values = [
1178 problem.variable(id_i).unwrap().value(),
1179 problem.variable(id_j).unwrap().value(),
1180 problem.variable(bias_id).unwrap().value(),
1181 ];
1182 let evaluation = factor.evaluate(&values).unwrap();
1183 assert!(evaluation.residual.norm() < 1.0e-9);
1184 assert_eq!(evaluation.jacobians[0].shape(), (9, 9));
1185 assert_eq!(evaluation.jacobians[2].shape(), (9, 6));
1186 for variable in 0..3 {
1187 let numerical = factor.numerical_jacobian(&values, variable);
1188 assert!(
1189 (&evaluation.jacobians[variable] - numerical).norm() < 1.0e-5,
1190 "analytic IMU Jacobian {variable} disagrees with finite differences"
1191 );
1192 }
1193 }
1194
1195 #[test]
1196 fn trajectory_optimizer_preserves_consistent_prediction() {
1197 let bias = ImuBias {
1198 accelerometer: Vector3::new(0.01, -0.02, 0.03),
1199 gyroscope: Vector3::new(0.001, -0.002, 0.003),
1200 };
1201 let gravity = Vector3::new(0.0, 0.0, -9.81);
1202 let initial = NavState::identity();
1203 let mut measurement = PreintegratedImuMeasurement::new(bias, ImuNoise::default());
1204 measurement
1205 .integrate(
1206 Vector3::new(0.1, -0.2, 9.9),
1207 Vector3::new(0.02, 0.01, -0.03),
1208 0.1,
1209 )
1210 .unwrap();
1211 let predicted = measurement.predict(&initial, &bias, gravity);
1212 let result = optimize_imu_trajectory(
1213 &[initial.clone(), predicted.clone()],
1214 bias,
1215 &[measurement],
1216 gravity,
1217 &ImuTrajectoryConfig::default(),
1218 )
1219 .unwrap();
1220 assert!(nav_state_local(&result.states[0], &initial).norm() < 1.0e-9);
1221 assert!(nav_state_local(&result.states[1], &predicted).norm() < 1.0e-9);
1222 assert!((result.biases[0].vector() - bias.vector()).norm() < 1.0e-9);
1223 assert!((result.biases[1].vector() - bias.vector()).norm() < 1.0e-9);
1224 }
1225
1226 #[test]
1227 fn visual_state_priors_make_constant_accelerometer_bias_observable() {
1228 let gravity = Vector3::new(0.0, 0.0, -9.81);
1229 let true_bias = ImuBias {
1230 accelerometer: Vector3::new(0.2, -0.1, 0.05),
1231 gyroscope: Vector3::zeros(),
1232 };
1233 let linearization_bias = ImuBias::zero();
1234 let initial = NavState::identity();
1235 let mut states = vec![initial.clone()];
1236 let mut measurements = Vec::new();
1237 for _ in 0..4 {
1238 let mut measurement =
1239 PreintegratedImuMeasurement::new(linearization_bias, ImuNoise::default());
1240 for _ in 0..10 {
1241 measurement
1242 .integrate(
1243 Vector3::new(0.0, 0.0, 9.81) + true_bias.accelerometer,
1244 Vector3::zeros(),
1245 0.01,
1246 )
1247 .unwrap();
1248 }
1249 states.push(measurement.predict(states.last().unwrap(), &true_bias, gravity));
1250 measurements.push(measurement);
1251 }
1252 let mut visual_information = Matrix9::zeros();
1253 visual_information
1254 .fixed_view_mut::<6, 6>(0, 0)
1255 .copy_from(&(SMatrix::<f64, 6, 6>::identity() * 1.0e8));
1256 let result = optimize_imu_trajectory(
1257 &states,
1258 linearization_bias,
1259 &measurements,
1260 gravity,
1261 &ImuTrajectoryConfig {
1262 nav_measurement_information: Some(visual_information),
1263 bias_prior_information: Matrix6::identity() * 1.0e-6,
1264 bias_between_information: Matrix6::identity() * 1.0e6,
1265 ..ImuTrajectoryConfig::default()
1266 },
1267 )
1268 .unwrap();
1269 assert!(
1270 (result.biases[0].accelerometer - true_bias.accelerometer).norm() < 1.0e-4,
1271 "estimated bias {:?}",
1272 result.biases[0].accelerometer
1273 );
1274 }
1275}