Suggestion: add error metrics

[VRichardJP]

DLIO lacks a good way to measure mapping accuracy. I think position and orientation error calculated by updateState could make a good metric. If the geometric observer converged, then the following metrics should have low values:

// estimated_pose: estimated pose prior GICP alignment
// true_pose: GICP-aligned pose

// position error
Eigen::Vector3f pe = true_pose.p - estimated_pose.p;
// orientation error
Eigen::Quaternionf qe = estimated_pose.q.conjugate() * true_pose.q;

// "angular amplitude" of the orientation error
double ang_err_rad = 2. * std::atan2(qe.vec().norm(), qe.w());
double ang_err_deg = ang_err_rad * (180.0 / M_PI);

// distance error
double dist_err = pe.norm();

For example:

|===================================================================|
| Position     {W}  [xyz] :: 0.0054 -0.0050 0.0000                  |
| Orientation  {W} [wxyz] :: 0.9999 0.0074 -0.0104 -0.0000          |
| Lin Velocity {B}  [xyz] :: -0.0020 -0.0102 -0.0000                |
| Ang Velocity {B}  [xyz] :: 0.0007 0.0019 -0.0022                  |
| Accel Bias        [xyz] :: 0.00399720 0.00026239 -0.00016252      |
| Gyro Bias         [xyz] :: 0.00039962 0.00023524 0.00010726       |
| Position err     [dxyz] :: 0.0078 -0.0061 0.0049 0.0000           |  <--- estimated position error
| Orientation err   [deg] :: 0.0269                                 |  <--- estimated orientation error
|                                                                   |
| Distance Traveled  :: 0.0457 meters                               |
| Distance to Origin :: 0.0074 meters                               |
| Registration       :: keyframes: 1, deskewed points: 57417        |
|                                                                   |