compose_Rt() has new args: inverted0,inverted1

BREAKING C ABI,API change: mrcal_compose_Rt_full() takes the two new arguments.
The mrcal_compose_Rt() macro and the Python functions maintain the same
interface.

doc/news-3.0.org

@@ -50,15 +50,16 @@ faster uncertainty computation:
   - MUCH slower than the mean-pcam, since it solves the linear problem (Nframes
     or Ncameras_extrinsics) more times than mean-pcam
 
-2024-12-06 compose_r() has new args: inverted0,inverted1
+2024-12-06 compose_r(), compose_rt(), compose_Rt() has new args:
+inverted0,inverted1
 
 * Migration notes 2.4 -> 3.0
 
-2024-12-06 compose_r(), compose_rt() have new args: inverted0,inverted1 BREAKING
-  C ABI,API change: mrcal_compose_r_full(), mrcal_compose_rt_full() takes the
-  two new arguments. And mrcal_compose_rt_full() returns dt01/dr1 and dt01/dt0.
-  The mrcal_compose_r(), mrcal_compose_rt() macros and the Python functions
-  maintain the same interface
+2024-12-06 compose_r(), compose_rt(), compose_Rt() have new args:
+  inverted0,inverted1 BREAKING C ABI,API change: mrcal_compose_..._full() take
+  the two new arguments. And mrcal_compose_rt_full() returns dt01/dr1 and
+  dt01/dt0. The mrcal_compose_r(), mrcal_compose_rt() macros and the Python
+  functions maintain the same interface
 
 * todo
 - Old tools complain about new keywords:

mrcal/poseutils.py

@@ -551,7 +551,7 @@ def invert_rt(rt, *, get_gradients=False, out=None):
         return _poseutils_npsp._invert_rt_withgrad(rt, out=out)
     return _poseutils_npsp._invert_rt(rt, out=out)
 
-def compose_Rt(*Rt, out=None):
+def compose_Rt(*Rt, out=None, inverted0=False, inverted1=False):
     r"""Compose Rt transformations
 
 SYNOPSIS
@@ -601,6 +601,10 @@ def compose_Rt(*Rt, out=None):
   transformations, but any number could be given here. Each broadcasted slice
   has shape (4,3).
 
+- inverted0,inverted1: optional booleans, defaulting to False. If True, the
+  opposite transform is used for Rt0 and/or Rt1 respectively. inverted=True is
+  only supported when exactly two transforms are given
+
 - out: optional argument specifying the destination. By default, a new numpy
   array is created and returned. To write the results into an existing (and
   possibly non-contiguous) array, specify it with the 'out' kwarg. If 'out' is
@@ -612,6 +616,12 @@ def compose_Rt(*Rt, out=None):
 An array of composed Rt transformations. Each broadcasted slice has shape (4,3)
 
     """
+    if len(Rt) == 2:
+        return _poseutils_npsp._compose_Rt(*Rt, out=out, inverted0=inverted0, inverted1=inverted1)
+
+    if inverted0 or inverted1:
+        raise Exception("compose_Rt(..., inverted...=True) is supported only if exactly 2 inputs are given")
+
     Rt1onwards = reduce( _poseutils_npsp._compose_Rt, Rt[1:] )
     return _poseutils_npsp._compose_Rt(Rt[0], Rt1onwards, out=out)
 

poseutils-genpywrap.py

@@ -685,6 +685,8 @@
             args_input       = ('Rt0', 'Rt1'),
             prototype_input  = ((4,3,), (4,3,)),
             prototype_output = (4,3),
+            extra_args = (("int", "inverted0", "false", "p"),
+                          ("int", "inverted1", "false", "p"),),
 
             Ccode_slice_eval = \
                 {np.float64:
@@ -694,7 +696,8 @@
                            (const double*)data_slice__Rt0,
                            strides_slice__Rt0[0], strides_slice__Rt0[1],
                            (const double*)data_slice__Rt1,
-                           strides_slice__Rt1[0], strides_slice__Rt1[1] );
+                           strides_slice__Rt1[0], strides_slice__Rt1[1],
+                           *inverted0, *inverted1);
     return true;
 '''},
 )

poseutils.c

@@ -165,6 +165,60 @@ void mul_gen33_gen33_vout_full(// output
         for(int j=0; j<3; j++)
             P2(m0m1, i,j) = outcopy2[3*i+j];
 }
+static inline
+void mul_gen33t_gen33_vout_full(// output
+                                double* m0m1,
+                                int m0m1_stride0, int m0m1_stride1,
+
+                                // input
+                                const double* m0,
+                                int m0_stride0, int m0_stride1,
+                                const double* m1,
+                                int m1_stride0, int m1_stride1)
+{
+    mul_gen33_gen33_vout_full(m0m1,
+                              m0m1_stride0, m0m1_stride1,
+                              m0,
+                              m0_stride1, m0_stride0,
+                              m1,
+                              m1_stride0, m1_stride1);
+}
+static inline
+void mul_gen33_gen33t_vout_full(// output
+                                double* m0m1,
+                                int m0m1_stride0, int m0m1_stride1,
+
+                                // input
+                                const double* m0,
+                                int m0_stride0, int m0_stride1,
+                                const double* m1,
+                                int m1_stride0, int m1_stride1)
+{
+    mul_gen33_gen33_vout_full(m0m1,
+                              m0m1_stride0, m0m1_stride1,
+                              m0,
+                              m0_stride0, m0_stride1,
+                              m1,
+                              m1_stride1, m1_stride0);
+}
+static inline
+void mul_gen33t_gen33t_vout_full(// output
+                                double* m0m1,
+                                int m0m1_stride0, int m0m1_stride1,
+
+                                // input
+                                const double* m0,
+                                int m0_stride0, int m0_stride1,
+                                const double* m1,
+                                int m1_stride0, int m1_stride1)
+{
+    mul_gen33_gen33_vout_full(m0m1,
+                              m0m1_stride0, m0m1_stride1,
+                              m0,
+                              m0_stride1, m0_stride0,
+                              m1,
+                              m1_stride1, m1_stride0);
+}
 
 static inline
 double inner3(const double* restrict a,
@@ -557,31 +611,130 @@ void mrcal_compose_Rt_full( // output
                            int Rt_0_stride1,    // in bytes. <= 0 means "contiguous"
                            const double* Rt_1,  // (4,3) array
                            int Rt_1_stride0,    // in bytes. <= 0 means "contiguous"
-                           int Rt_1_stride1     // in bytes. <= 0 means "contiguous"
-                            )
+                           int Rt_1_stride1,    // in bytes. <= 0 means "contiguous"
+                           bool inverted0,
+                           bool inverted1)
 {
     init_stride_2D(Rt_out, 4,3);
     init_stride_2D(Rt_0,   4,3);
     init_stride_2D(Rt_1,   4,3);
 
-    // for in-place operation
-    double t0[] = { P2(Rt_0,3,0),
-                    P2(Rt_0,3,1),
-                    P2(Rt_0,3,2) };
 
-    // t <- R0*t1
-    mul_vec3_gen33t_vout_full(&P2(Rt_out,3,0), Rt_out_stride1,
-                              &P2(Rt_1,  3,0), Rt_1_stride1,
-                              Rt_0, Rt_0_stride0, Rt_0_stride1);
+    /*
+      I have 4 cases based on the values of inverted0,inverted1. Nominally we have:
 
-    // R <- R0*R1
-    mul_gen33_gen33_vout_full( Rt_out, Rt_out_stride0, Rt_out_stride1,
-                               Rt_0,   Rt_0_stride0,   Rt_0_stride1,
-                               Rt_1,   Rt_1_stride0,   Rt_1_stride1 );
+         R0 R1 x + R0 t1 + t0
 
-    // t <- R0*t1+t0
-    for(int i=0; i<3; i++)
-        P2(Rt_out,3,i) += t0[i];
+      -> R01 = R0 R1
+         t01 = R0 t1 + t0
+
+      If we invert anything we use the inverted transform for r,t:
+
+         r x + t = y -> x = Rt y - Rt t
+      -> r becomes Rt, t becomes -Rt t
+
+      So
+      inverted0:
+         R01 = R0t R1
+         t01 = R0t t1 - R0t t0
+             = R0t (t1-t0)
+
+      inverted1:
+         R01 = R0 R1t
+         t01 = -R0 R1t t1 + t0
+
+      inverted01:
+         R01 = R0t R1t
+         t01 = -R0t R1t t1 - R0t t0
+    */
+
+    if(!inverted0 && !inverted1)
+    {
+        // R01 = R0 R1
+        // t01 = R0 t1 + t0
+
+        // for in-place operation
+        const double t0[] = { P2(Rt_0,3,0),
+                              P2(Rt_0,3,1),
+                              P2(Rt_0,3,2) };
+
+        // t <- R0*t1
+        mul_vec3_gen33t_vout_full(&P2(Rt_out,3,0), Rt_out_stride1,
+                                  &P2(Rt_1,  3,0), Rt_1_stride1,
+                                  Rt_0, Rt_0_stride0, Rt_0_stride1);
+        // R <- R0*R1
+        mul_gen33_gen33_vout_full( Rt_out, Rt_out_stride0, Rt_out_stride1,
+                                   Rt_0,   Rt_0_stride0,   Rt_0_stride1,
+                                   Rt_1,   Rt_1_stride0,   Rt_1_stride1 );
+        // t <- R0*t1+t0
+        for(int i=0; i<3; i++)
+            P2(Rt_out,3,i) += t0[i];
+    }
+    else if(inverted0 && !inverted1)
+    {
+        // R01 = R0t R1
+        // t01 = R0t t1 - R0t t0
+        //     = R0t (t1-t0)
+        const double t10[] = { P2(Rt_1,3,0) - P2(Rt_0,3,0),
+                               P2(Rt_1,3,1) - P2(Rt_0,3,1),
+                               P2(Rt_1,3,2) - P2(Rt_0,3,2) };
+
+        // t <- R0t*(t1-t0)
+        mul_vec3_gen33_vout_full(&P2(Rt_out,3,0), Rt_out_stride1,
+                                 t10, sizeof(t10[0]),
+                                 Rt_0, Rt_0_stride0, Rt_0_stride1);
+        // R <- R0t*R1
+        mul_gen33t_gen33_vout_full( Rt_out, Rt_out_stride0, Rt_out_stride1,
+                                    Rt_0,   Rt_0_stride0,   Rt_0_stride1,
+                                    Rt_1,   Rt_1_stride0,   Rt_1_stride1 );
+    }
+    else if(!inverted0 && inverted1)
+    {
+        // R01 = R0 R1t
+        // t01 = -R0 R1t t1 + t0
+
+        // for in-place operation
+        const double t0[] = { P2(Rt_0,3,0),
+                              P2(Rt_0,3,1),
+                              P2(Rt_0,3,2) };
+
+        // R <- R0*R1t
+        mul_gen33_gen33t_vout_full( Rt_out, Rt_out_stride0, Rt_out_stride1,
+                                    Rt_0,   Rt_0_stride0,   Rt_0_stride1,
+                                    Rt_1,   Rt_1_stride0,   Rt_1_stride1 );
+
+        // t01 <- R0 R1t t1
+        mul_vec3_gen33t_vout_full(&P2(Rt_out,3,0), Rt_out_stride1,
+                                  &P2(Rt_1,  3,0), Rt_1_stride1,
+                                  &P2(Rt_out,0,0), Rt_out_stride0, Rt_out_stride1);
+
+        // t01 <- -R0 R1t t1 + t0
+        for(int i=0; i<3; i++)
+            P2(Rt_out,3,i) = -P2(Rt_out,3,i) + t0[i];
+    }
+    else
+    {
+        // R01 = R0t R1t
+        // t01 = -R0t R1t t1 - R0t t0
+        const double R0t_t0[3];
+        mul_vec3_gen33_vout_full(R0t_t0, sizeof(R0t_t0[0]),
+                                 &P2(Rt_0,  3,0), Rt_0_stride1,
+                                 &P2(Rt_0,0,0), Rt_0_stride0, Rt_0_stride1);
+
+        // R <- R0t*R1t
+        mul_gen33t_gen33t_vout_full( Rt_out, Rt_out_stride0, Rt_out_stride1,
+                                     Rt_0,   Rt_0_stride0,   Rt_0_stride1,
+                                     Rt_1,   Rt_1_stride0,   Rt_1_stride1 );
+
+        // t01 <- R0t R1t t1
+        mul_vec3_gen33t_vout_full(&P2(Rt_out,3,0), Rt_out_stride1,
+                                  &P2(Rt_1,  3,0), Rt_1_stride1,
+                                  &P2(Rt_out,0,0), Rt_out_stride0, Rt_out_stride1);
+
+        // t01 <- -R0t R1t t1 - R0t t0
+        for(int i=0; i<3; i++)
+            P2(Rt_out,3,i) = -P2(Rt_out,3,i) - R0t_t0[i];
+    }
 }
 
 // Compose two rt transformations. It is assumed that we're getting no gradients

poseutils.h

@@ -406,7 +406,10 @@ void mrcal_invert_rt_full( // output
 //
 // In-place operation is supported; the output array may be the same as either
 // of the input arrays to overwrite the input.
-#define mrcal_compose_Rt(Rt_out,Rt_0,Rt_1) mrcal_compose_Rt_full(Rt_out,0,0,Rt_0,0,0,Rt_1,0,0)
+#define mrcal_compose_Rt(           Rt_out,Rt_0,Rt_1) mrcal_compose_Rt_full(Rt_out,0,0,Rt_0,0,0,Rt_1,0,0,false,false)
+#define mrcal_compose_Rt_inverted0( Rt_out,Rt_0,Rt_1) mrcal_compose_Rt_full(Rt_out,0,0,Rt_0,0,0,Rt_1,0,0,true, false)
+#define mrcal_compose_Rt_inverted1( Rt_out,Rt_0,Rt_1) mrcal_compose_Rt_full(Rt_out,0,0,Rt_0,0,0,Rt_1,0,0,false,true )
+#define mrcal_compose_Rt_inverted01(Rt_out,Rt_0,Rt_1) mrcal_compose_Rt_full(Rt_out,0,0,Rt_0,0,0,Rt_1,0,0,true, true )
 void mrcal_compose_Rt_full( // output
                            double* Rt_out,      // (4,3) array
                            int Rt_out_stride0,  // in bytes. <= 0 means "contiguous"
@@ -418,8 +421,9 @@ void mrcal_compose_Rt_full( // output
                            int Rt_0_stride1,    // in bytes. <= 0 means "contiguous"
                            const double* Rt_1,  // (4,3) array
                            int Rt_1_stride0,    // in bytes. <= 0 means "contiguous"
-                           int Rt_1_stride1     // in bytes. <= 0 means "contiguous"
-                           );
+                           int Rt_1_stride1,    // in bytes. <= 0 means "contiguous"
+                           bool inverted0,
+                           bool inverted1);
 
 // Compose two rt transformations
 //

test/test-poseutils-lib.py

@@ -786,28 +786,56 @@
                drt_drt_ref,
                msg='invert_rt with grad drt/drt result written in-place')
 
+############ compose_Rt()
+
 Rt2 = mrcal.compose_Rt(Rt0_ref, Rt1_ref,
                        out=out43)
 confirm_equal( Rt2,
                compose_Rt(Rt0_ref, Rt1_ref),
                msg='compose_Rt result')
 
 # in-place
-Rt0_ref_copy = np.array(Rt0_ref)
-Rt1_ref_copy = np.array(Rt1_ref)
-Rt2 = mrcal.compose_Rt(Rt0_ref_copy, Rt1_ref_copy,
-                       out=Rt0_ref_copy)
-confirm_equal( Rt2,
-               compose_Rt(Rt0_ref, Rt1_ref),
-               msg='compose_Rt result written in-place to Rt0')
-Rt0_ref_copy = np.array(Rt0_ref)
-Rt1_ref_copy = np.array(Rt1_ref)
-Rt2 = mrcal.compose_Rt(Rt0_ref_copy, Rt1_ref_copy,
-                       out=Rt1_ref_copy)
-confirm_equal( Rt2,
-               compose_Rt(Rt0_ref, Rt1_ref),
-               msg='compose_Rt result written in-place to Rt1')
-
+for iout,outname in ( (0, "Rt0"),
+                      (1, "Rt1"),):
+    Rt0_ref_copy = np.array(Rt0_ref)
+    Rt1_ref_copy = np.array(Rt1_ref)
+    out = (Rt0_ref_copy,Rt1_ref_copy)[iout]
+    Rt2 = mrcal.compose_Rt(Rt0_ref_copy, Rt1_ref_copy,
+                           out=out)
+    confirm_equal( Rt2,
+                   compose_Rt(Rt0_ref, Rt1_ref),
+                   msg=f'compose_Rt result written in-place to {outname}')
+
+    Rt0_ref_copy = np.array(Rt0_ref)
+    Rt1_ref_copy = np.array(Rt1_ref)
+    out = (Rt0_ref_copy,Rt1_ref_copy)[iout]
+    Rt2 = mrcal.compose_Rt(Rt0_ref_copy, Rt1_ref_copy,
+                           inverted0=True,
+                           out=out)
+    confirm_equal( Rt2,
+                   compose_Rt(invert_Rt(Rt0_ref), Rt1_ref),
+                   msg=f'compose_Rt result written in-place to {outname}: inverted0')
+
+    Rt0_ref_copy = np.array(Rt0_ref)
+    Rt1_ref_copy = np.array(Rt1_ref)
+    out = (Rt0_ref_copy,Rt1_ref_copy)[iout]
+    Rt2 = mrcal.compose_Rt(Rt0_ref_copy, Rt1_ref_copy,
+                           inverted1=True,
+                           out=out)
+    confirm_equal( Rt2,
+                   compose_Rt(Rt0_ref, invert_Rt(Rt1_ref)),
+                   msg=f'compose_Rt result written in-place to {outname}: inverted1')
+
+    Rt0_ref_copy = np.array(Rt0_ref)
+    Rt1_ref_copy = np.array(Rt1_ref)
+    out = (Rt0_ref_copy,Rt1_ref_copy)[iout]
+    Rt2 = mrcal.compose_Rt(Rt0_ref_copy, Rt1_ref_copy,
+                           inverted0=True,
+                           inverted1=True,
+                           out=out)
+    confirm_equal( Rt2,
+                   compose_Rt(invert_Rt(Rt0_ref), invert_Rt(Rt1_ref)),
+                   msg=f'compose_Rt result written in-place to {outname}: inverted01')
 
 
 ############ compose_rt()