Wheeled Odometry Calibration Setup Examples

doc/installation.md

@@ -52,7 +52,7 @@ The scripts and commands below invoke `wget, git, add-apt-repository` which may
 > **Cmake Note**: certain librealsense CMAKE flags (e.g. CUDA) require version 3.8+ which is currently not made available via apt manager for Ubuntu LTS.   
     Go to the [official CMake site](https://cmake.org/download/) to download and install the application  
 
-     **Note** on graphic sub-system utilization:<br />
+  **Note** on graphic sub-system utilization:<br />
      *glfw3*, *mesa* and *gtk* packages are required if you plan to build the SDK's OpenGl-enabled examples. The *librealsense* core library and a range of demos/tools are designed for headless environment deployment.
 
   3. Run Intel Realsense permissions script located from librealsense root directory:<br />

doc/t265.md

@@ -160,7 +160,7 @@ Yes, there are.
 ## Appendix
 
 ### Wheel odometry calibration file format
-Below is a sample calibration file with inline comments (not a valid file format!) that explain each field. Please note that this is an intermediate solution and a more unified interface will be coming.
+Below is a sample calibration file with inline comments (not a valid file format! Remove "//comments" to make valid) that explain each field. Please note that this is an intermediate solution and a more unified interface will be coming.
 ```
 {
     "velocimeters": [ // array of velocity sensors (currently max. 2 supported)
@@ -202,3 +202,107 @@ Below is a sample calibration file with inline comments (not a valid file format
         }
     ]
 }
+```
+### Extrinsic Calibration for Wheeled Odometry Examples
+
+ - All calibration metrics are relative to T265 origin frame. I.e.: They are offsets/rotations *from* the T265 origin *to* the robot's origin. Said another way, we transform a point from frame B (Robot Odometry) to frame A (T265 Pose) or A_p = H_AB * B_p, where A_p is the point expressed in frame A, B_p is the point expressed in frame B, and H_AB is the corresponding homogeneous transformation.
+
+ - In order for the T265 to consume and use odometry data correctly it must know where the robot's origin is relative to itself. This is accomplished by feeding a calibration (json) file into the librealsense API with this initial data.
+
+
+ In this basic sample robot setup, for simplicity, we assume the X-axis of the camera and robot's coordinate system are aligned (+X coming out of the screen) and only concern ourselves with the Y and Z axis. 
+
+![T265 Robot 1](./img/T265_Robot1.png)
+
+ Corresponsing JSON:
+ ```
+ {
+    "velocimeters": [
+        {
+            "scale_and_alignment": [
+                1.0,
+                0.0,
+                0.0,
+                0.0,
+                1.0,
+                0.0,
+                0.0,
+                0.0,
+                1.0
+            ],
+            "noise_variance": 0.00004445126050420168,
+            "extrinsics": {
+                "T": [
+                    0.0,      // No Translation.
+                    -0.92,   // 0.92m below (-Y) the camera.
+                    0.44    // 0.44m behind (+Z) the camera.
+                ],
+                "T_variance": [
+                    9.999999974752427e-7,
+                    9.999999974752427e-7,
+                    9.999999974752427e-7
+                ],
+                "W": [
+                    0.0,
+                    0.0,
+                    0.0
+                ],
+                "W_variance": [
+                    9.999999974752427e-5,
+                    9.999999974752427e-5,
+                    9.999999974752427e-5
+                ]
+            }
+        }
+    ]
+}
+```
+
+ In this example, we take the above setup and add a rotation around the camera's X-axis.
+
+![T265 Robot 2](./img/T265_Robot2.png)
+
+ Corresponding JSON:
+
+ ```
+ {
+    "velocimeters": [
+        {
+            "scale_and_alignment": [
+                1.0,
+                0.0,
+                0.0,
+                0.0,
+                1.0,
+                0.0,
+                0.0,
+                0.0,
+                1.0
+            ],
+            "noise_variance": 0.00004445126050420168,
+            "extrinsics": {
+                "T": [
+                    0.0,      // No Translation.
+                    -0.92,   // 0.92m below (-Y) the camera.
+                    0.44    // 0.44m behind (+Z) the camera.
+                ],
+                "T_variance": [
+                    9.999999974752427e-7,
+                    9.999999974752427e-7,
+                    9.999999974752427e-7
+                ],
+                "W": [
+                    -0.7853982,  // -0.78rad (-pi/4) around the camera x-axis.
+                    0.0,
+                    0.0
+                ],
+                "W_variance": [
+                    9.999999974752427e-5,
+                    9.999999974752427e-5,
+                    9.999999974752427e-5
+                ]
+            }
+        }
+    ]
+}
+ ```