Fix metrics bug and update sophuspy installation instructions

Author: k-sheridan

README.md

@@ -37,3 +37,11 @@ The most complete example on how to use ArgMin is in this test: [TestArgMinExamp
 ## ArgMin: Documentation
 The most complete documentation of ArgMin's SSEOptimizer can be found [here](docs/source/SSEOptimizer.md)
 
+## Running QDVO
+
+### Running Tests
+To build and run QDVO's unit tests and benchmarks, see the [test README](test/README.md) for detailed Docker-based instructions.
+
+### Running Evaluation
+To evaluate QDVO on EuRoC format datasets and compute trajectory accuracy metrics, see the [tools README](tools/README.md) for evaluation scripts and usage instructions.
+

tools/README.md

@@ -61,13 +61,17 @@ cmake --build build
 # Install basic Python packages
 pip3 install --break-system-packages numpy scipy termcolor
 
-# Build and install sophuspy from source
+# Build and install sophuspy from source with explicit compiler settings
 git clone https://github.com/craigstar/SophusPy
 cd SophusPy
+export CC=clang
+export CXX=clang++
 pip3 install --break-system-packages .
 cd ..
 ```
 
+**Note:** Setting `CC` and `CXX` to use clang ensures sophuspy is compiled with the same compiler as QDVO, preventing ABI compatibility issues that can cause segmentation faults.
+
 ### 6. Run Evaluation
 
 ```bash

tools/sim3TrajectoryError.py

@@ -7,7 +7,7 @@
 
 
 def quaternionToSO3(w, x, y, z):
-    r = R.from_quat([w, x, y, z])
+    r = R.from_quat([x, y, z, w])
     return sophus.SO3(np.array(r.as_matrix()))
 
 
@@ -78,15 +78,15 @@ def computeTrajectoryError(trajectory1, trajectory2):
     # dR(phi)_dphi = Jrinv((R1.inverse() * R_sim * R2).log()) * R2.inverse()
 
     # Order: [translation, rotation, scale]
-    R_sim = sophus.SO3.exp([0, 0, 0])
+    R_sim = sophus.SO3.exp(np.array([0.0, 0.0, 0.0]))
     t_sim = np.zeros([3, 1])
     scale = 1.0
 
     # dx order: [translation, rotation, scale]
     # output: [rotation, translation, scale]
     def applyUpdate(R_sim, t_sim, scale, dx):
         return R_sim * sophus.SO3.exp(
-            dx[3:6, 0:1]), t_sim + dx[0:3, 0:1], scale + dx[6:7, 0:1]
+            dx[3:6, 0]), t_sim + dx[0:3, 0:1], scale + dx[6, 0]
 
     # output: [dt, dR]
     def computeError(R1, t1, R2, t2, R_sim, t_sim, scale):
@@ -217,7 +217,7 @@ def computeOdometryError():
     rotErrorPerFrame, transErrorPerFrame, bearingErrorPerFrame, scaleErrorPerFrame = computeOdometryError(
     )
     return posRMSE, rotRMSE, abs(
-        scale[0, 0]
+        scale
     ), rotErrorPerFrame, transErrorPerFrame, bearingErrorPerFrame, scaleErrorPerFrame