Experimental order-sensitive consistency residual for Odometry/TF streams — minimal C++ reproducer

ROS ROS General
,
1

I briefly mentioned an order-sensitive state diagnostic in another thread, but that was the wrong place for it. Posting it separately here with an executable reproducer.

The idea is simple: three consecutive pose samples in, one scalar residual out. It quantifies how much the result shifts when you change the nesting order of state composition.

Synthetic test results:

  • Smooth linear motion: 4.9848e-08
  • Abrupt pose/orientation jump: 0.000881456

This is absolutely not a validated anomaly detector yet. Normalization, frame conventions and real-world thresholds all need work.

No ROS, Eigen, or external dependencies required to run the reproducer.

#include <cmath>
#include <iostream>

struct Q { double w,x,y,z; };
Q qc(Q q){ return {q.w,-q.x,-q.y,-q.z}; }
Q qm(Q a,Q b){ return {
  a.w*b.w-a.x*b.x-a.y*b.y-a.z*b.z,
  a.w*b.x+a.x*b.w+a.y*b.z-a.z*b.y,
  a.w*b.y-a.x*b.z+a.y*b.w+a.z*b.x,
  a.w*b.z+a.x*b.y-a.y*b.x+a.z*b.w}; }
Q add(Q a,Q b){ return {a.w+b.w,a.x+b.x,a.y+b.y,a.z+b.z}; }
Q sub(Q a,Q b){ return {a.w-b.w,a.x-b.x,a.y-b.y,a.z-b.z}; }

struct State8 { Q a,b; };
State8 compose(State8 x, State8 y) {
  return {sub(qm(x.a,y.a), qm(qc(y.b),x.b)),
          add(qm(y.b,x.a), qm(x.b,qc(y.a)))};
}

struct Pose { double x,y,z,qw,qx,qy,qz; };
State8 encode(Pose p) {
  return {{p.qw,p.qx,p.qy,p.qz},{p.x,p.y,p.z,0.0}};
}

double order_sensitive_residual(Pose A,Pose B,Pose C) {
  State8 x=compose(compose(encode(A),encode(B)),encode(C));
  State8 y=compose(encode(A),compose(encode(B),encode(C)));
  double d[8]={x.a.w-y.a.w,x.a.x-y.a.x,x.a.y-y.a.y,x.a.z-y.a.z,
               x.b.w-y.b.w,x.b.x-y.b.x,x.b.y-y.b.y,x.b.z-y.b.z};
  double s=0; for(double v:d) s+=v*v;
  return std::sqrt(s);
}

int main() {
  Pose a{.1,.001,0,.99875026,0,0,.04997917};
  Pose b{.2,.004,0,.99500417,0,0,.09983342};
  Pose c{.3,.009,0,.98877108,0,0,.14943813};

  std::cout << "smooth: " << order_sensitive_residual(a,b,c) << '\n';

  c.z=2.0; c.qw=.92106099; c.qx=.38941834; c.qy=c.qz=0;
  std::cout << "jump:   " << order_sensitive_residual(a,b,c) << '\n';
}