offset mode: fix wobble when rotating head

src/calibrator/offset.rs

@@ -138,7 +138,6 @@ impl Calibrator for OffsetMethod {
  let to_b = data.get_device_origin(self.device_b)?;
  let root_b = TransformD::from(to_b.get_offset()?);
 
- //let pos_offset = root_b.origin - (target_a.origin - pose_a.origin);
  let pos_offset = root_b.origin + delta_global.origin;
 
  // devices are more than 100m from center → anomaly

src/calibrator/sampled.rs

@@ -218,15 +218,14 @@ impl SampledMethod {
  /// informed by "Averaging Quaternions" from F. Landis Markley, Yang Cheng, John
  /// L. Crassidis, Yaakov Oshman
  /// https://www.acsu.buffalo.edu/%7Ejohnc/ave_quat07.pdf
- fn avg_b_to_a_offset(&self, offset: TransformD) -> TransformD {
- let mut verts = Vector3::zeros();
+ fn avg_b_to_a_offset(&self) -> TransformD {
+ let mut vecs = Vector3::zeros();
  let mut quats = Matrix4::zeros();
 
  for samp in self.samples.iter() {
- //TODO:validate
- let delta = (offset * samp.b).inverse() * samp.a;
+ let delta = samp.b.inverse() * samp.a;
 
- verts += delta.origin;
+ vecs += delta.origin;
 
  let mut q = UnitQuaternion::from_rotation_matrix(&delta.basis);
  if q.w < 0.0 {
@@ -237,7 +236,7 @@ impl SampledMethod {
  quats += v * v.adjoint();
  }
 
- let out_pos = verts.scale(1.0 / self.samples.len() as f64);
+ let out_pos = vecs.scale(1.0 / self.samples.len() as f64);
 
  let eigen = quats.symmetric_eigen();
  let e0 = eigen.eigenvectors.column(0);
@@ -306,7 +305,7 @@ impl Calibrator for SampledMethod {
  dst_origin.set_offset((offset * dst_root).into())?;
 
  if self.maintain {
- let offset = self.avg_b_to_a_offset(offset);
+ let offset = self.avg_b_to_a_offset();
 
  match data.save_calibration(self.src_dev, self.dst_dev, offset, OffsetType::Device) {
  Ok(_) => log::info!(