Correct the order of multiplication in the extrinsic graph

[dmirota]

When traversing a pose graph, the most recently traversed pose should be right-hand multiplied.

Without this fix the extrinsics of the T265 are incorrect if they are express in a frame other than the pose stream. The translation between the gyro and fisheye 1 is ~10mm. Before this fix: rs-enumerate-devices -c returns:

Extrinsic from "Fisheye 1"	  To	  "Gyro" :
 Rotation Matrix:
  -0.999942        -0.00747438       0.00777465    
   0.00745392      -0.999969        -0.00265752    
   0.00779427      -0.00259942       0.999966      

 Translation Vector: -0.0533220693469048  0.000280210515484214  0.0002495645894669  

After the fix rs-enumerate-devices -c returns the correct value closer to ~10mm

Extrinsic from "Fisheye 1"	  To	  "Gyro" :
 Rotation Matrix:
  -0.999942         0.00747438       0.00777465    
  -0.00745392      -0.999969         0.00265752    
   0.00779427       0.00259942       0.999966      

 Translation Vector: 0.0106993783265352  7.97569591668434e-05  -8.33986996440217e-05  

[radfordi]

Thanks @dmirota!

[dmirota]

Example output from rs-enumerate-devices -c of the changes to the extrinsics of D415 test_original.txt test_fix.txt
Diff

--- C:\dev\librealsense-build\Debug\test_org.txt	2019-05-03 13:16:24.000000000 -0700
+++ C:\dev\librealsense-build\Debug\test_fix.txt	2019-05-03 13:15:48.000000000 -0700
@@ -875,13 +875,13 @@
 Extrinsic from "Color"	  To	  "Infrared 2" :
  Rotation Matrix:
    0.999996        -0.00271892      -0.000997244   
    0.0027225        0.99999          0.00360575    
    0.00098743      -0.00360845       0.999993      
 
- Translation Vector: -0.0699888318777084  -0.000217952503589913  3.01693144137971e-05  
+ Translation Vector: -0.0699890628457069  -6.80379089317285e-05  8.4542203694582e-05  
 
 Extrinsic from "Infrared"	  To	  "Depth" :
  Rotation Matrix:
    1   0   0
    0   1   0
    0   0   1
@@ -971,13 +971,13 @@
 Extrinsic from "Infrared 2"	  To	  "Color" :
  Rotation Matrix:
    0.999996         0.0027225        0.00098743    
   -0.00271892       0.99999         -0.00360845    
   -0.000997244      0.00360575       0.999993      
 
- Translation Vector: 0.0699891000986099  2.77651524811517e-05  -9.91791457636282e-05  
+ Translation Vector: 0.0699888691306114  -0.000121952311019413  -0.000154092442244291  
 
 Extrinsic from "Infrared 2"	  To	  "Infrared" :
  Rotation Matrix:
    1   0   0
    0   1   0
    0   0   1

[dorodnic]

Thanks Dan!