correcting inverse and pose * operator

src/types.h

@@ -628,8 +628,8 @@ namespace librealsense
     inline float3x3 transpose(const float3x3 & a) { return{ {a.x.x,a.y.x,a.z.x}, {a.x.y,a.y.y,a.z.y}, {a.x.z,a.y.z,a.z.z} }; }
     inline bool operator == (const pose & a, const pose & b) { return a.orientation == b.orientation && a.position == b.position; }
     inline float3 operator * (const pose & a, const float3 & b) { return a.orientation * b + a.position; }
-    inline pose operator * (const pose & a, const pose & b) { return{ a.orientation * b.orientation, a * b.position }; }
-    inline pose inverse(const pose & a) { auto inv = transpose(a.orientation); return{ inv, inv * a.position * -1 }; }
+    inline pose operator * (const pose & a, const pose & b) { return{ a.orientation * b.orientation, a.position + b.position }; }
+    inline pose inverse(const pose & a) { auto inv = transpose(a.orientation); return{ inv, a.position * -1 }; }
     inline pose to_pose(const rs2_extrinsics& a)
     {
         pose r{};

unit-tests/func/extrinsics/test-extrinsics.cpp

@@ -22,12 +22,14 @@ using namespace rs2;
 // |     0     |      1       |
 // |_  (1x3)   |    (1x1)    _|
 //
+
+// rotation tolerance - units are in cosinus of radians
+const double rotation_tolerance = 0.000002;
+// translation tolerance - units are in meters
+const double translation_tolerance = 0.00105; // 1.05mm
+
 struct position_and_rotation {
     double pos_and_rot[4][4];
-    // rotation tolerance - units are in cosinus of radians
-    const double rotation_tolerance = 0.000002;
-    // translation tolerance - units are in meters
-    const double translation_tolerance = 0.0021; // 2.1mm
 
     position_and_rotation operator* (const position_and_rotation& other)
     {
@@ -160,7 +162,7 @@ TEST_CASE("Extrinsics graph - matrices 4x4", "[live]")
                 bool res = true;
                 for (int t = 0; t < 3; ++t)
                 {
-                    if (fabs(start_point[t] - end_point[t]) > 0.001f)
+                    if (fabs(start_point[t] - end_point[t]) > translation_tolerance)
                         res = false;
                 }
                 // checking that transforming a point with extrinsiscs from i to j and the from j to i