CODEX: Evaluate pose LMA candidate RMS on GPU

Co-authored-by: Codex <codex@elphel.com>

jna/test_pose_lma_resident_jna.cu

@@ -8,7 +8,7 @@
 #include <cstring>
 #include <vector>
 
-enum { NUM_PARAMS=3, NUM_COMPONENTS=2, PRODUCTS=12, RESULT=21 };
+enum { NUM_PARAMS=3, NUM_COMPONENTS=2, PRODUCTS=12, RESULT=23 };
 
 extern "C" {
   void* tp_create_module(const char*, const char*);
@@ -112,8 +112,56 @@ int main(int argc,char** argv)
     for(int i=0;i<numValues;i++)
       if(std::memcmp(preparedYmfx+i,expectedYmfx+i,sizeof(float))) preparedBad++;
     if(preparedBad){ std::printf("prepared bit mismatches=%d\n",preparedBad); bad++; }
-    if(packed[0]!=1.0f||!std::isfinite(packed[19])||!std::isfinite(packed[20])){
-      std::printf("invalid resident result valid=%g rms=%g/%g\n",packed[0],packed[19],packed[20]); bad++;
+    if(packed[0]!=1.0f||!std::isfinite(packed[19])||!std::isfinite(packed[20])||
+       !std::isfinite(packed[21])||!std::isfinite(packed[22])){
+      std::printf("invalid resident result valid=%g rms=%g/%g candidate=%g/%g\n",
+          packed[0],packed[19],packed[20],packed[21],packed[22]); bad++;
+    }
+
+    // Re-project the returned candidate through the public pose entry point and
+    // independently reproduce its float RMS. The resident call itself used
+    // separate candidate buffers, so this also checks that current raw state was
+    // not required for the compact candidate-RMS result.
+    float candidatePose[12]; std::memcpy(candidatePose,pose,sizeof(candidatePose));
+    for(int par=0;par<NUM_PARAMS;par++) candidatePose[9+par]=packed[16+par];
+    float candidateFx[2*numTiles],candidateJt[NUM_PARAMS*2*numTiles];
+    unsigned char candidateValid[numTiles];
+    const int candidateRc=tp_proc_exec_pose_fx_jacobian(
+        proc,meta,radial,rbr.data(),rbrLen,ers.data(),ersLen,
+        meta,radial,rbr.data(),rbrLen,ers.data(),ersLen,
+        candidatePose,centers,selection,numTiles,1,
+        candidateFx,candidateJt,candidateValid);
+    if(candidateRc){
+      std::fprintf(stderr,"candidate pose rc=%d: %s\n",candidateRc,tp_last_error()); bad++;
+    }else{
+      float pureL2=0.0f;
+      for(int tile=0;tile<numTiles;tile++){
+        const int vi=2*tile,ei=4*tile;
+        const bool use=weights[vi]>0.0f,have=use&&candidateValid[tile];
+        const float fx0=have?candidateFx[vi]:0.0f,fx1=have?candidateFx[vi+1]:0.0f;
+        float d0=0.0f,d1=0.0f;
+        if(use){
+          const float dx=fsub(y[vi],fx0),dy=fsub(y[vi+1],fx1);
+          d0=fadd(fmul(eigen[ei],dx),fmul(eigen[ei+1],dy));
+          d1=fadd(fmul(eigen[ei+2],dx),fmul(eigen[ei+3],dy));
+        }
+        pureL2=fadd(pureL2,fmul(d0,fmul(d0,weights[vi])));
+        pureL2=fadd(pureL2,fmul(d1,fmul(d1,weights[vi+1])));
+      }
+      float fullL2=pureL2;
+      for(int par=0;par<NUM_PARAMS;par++){
+        const int value=2*numTiles+par;
+        const float residual=fsub(y[value],packed[16+par]);
+        fullL2=fadd(fullL2,fmul(residual,fmul(residual,weights[value])));
+      }
+      const float expectedFull=std::sqrt(fullL2);
+      const float expectedPure=std::sqrt(pureL2/pureWeight);
+      const float maxRms=std::fmax(std::fabs(packed[21]-expectedFull),
+          std::fabs(packed[22]-expectedPure));
+      if(maxRms>2.0e-6f){
+        std::printf("candidate RMS mismatch max=%g expected=%g/%g got=%g/%g\n",
+            maxRms,expectedFull,expectedPure,packed[21],packed[22]); bad++;
+      }
     }
   }
   std::printf("NVRTC resident pose LMA: reused raw fx/J + prepared oracle + compact step -> %s\n",

src/tests/test_pose_lma_step.cu

@@ -253,6 +253,8 @@ int main(int argc, char ** argv)
 	checkCudaErrors(cudaGetLastError());
 	pose_lma_reduce_step<<<1, 1>>>(numPartials, lambda, pureWeight,
 			gpuPartials.dev(), gpuVectorResident.dev(), gpuResidentResult.dev());
+	pose_lma_reduce_rms<<<1, 1>>>(numPartials, pureWeight,
+			gpuPartials.dev(), gpuResidentResult.dev());
 	checkCudaErrors(cudaGetLastError()); checkCudaErrors(cudaDeviceSynchronize());
 	const std::vector<float> gotJt = gpuPreparedJt.download();
 	const std::vector<float> gotYmfx = gpuPreparedYmfx.download();
@@ -269,8 +271,10 @@ int main(int argc, char ** argv)
 		maxDelta = std::fmax(maxDelta, std::fabs(gotResident[13 + i] - expectedResident[13 + i]));
 		maxCandidate = std::fmax(maxCandidate, std::fabs(gotResident[16 + i] - expectedResident[16 + i]));
 	}
-	const float maxRms = std::fmax(std::fabs(gotResident[19] - expectedRms[0]),
+	float maxRms = std::fmax(std::fabs(gotResident[19] - expectedRms[0]),
 			std::fabs(gotResident[20] - expectedRms[1]));
+	maxRms = std::fmax(maxRms, std::fabs(gotResident[21] - expectedRms[0]));
+	maxRms = std::fmax(maxRms, std::fabs(gotResident[22] - expectedRms[1]));
 	const bool residentOk = (preparedBad == 0) && (gotResident[0] == 1.0f) &&
 			(maxProducts <= 0.02f) && (maxDelta <= 2.0e-5f) &&
 			(maxCandidate <= 2.0e-5f) && (maxRms <= 2.0e-6f);

src/tp_lma.cu

@@ -723,9 +723,31 @@ extern "C" __global__ void pose_lma_reduce_step(
 		values[2],values[4],values[5],
 		values[6],values[7],values[8]};
 	pose_lma_solve(lambda, products, vector, result);
-	result[19] = (values[10] >= 0.0f) ? sqrtf(values[10]) : nanf("");
-	result[20] = ((values[9] >= 0.0f) && (pure_weight > 0.0f)) ?
+	result[POSE_LMA_CURRENT_RMS] = (values[10] >= 0.0f) ? sqrtf(values[10]) : nanf("");
+	result[POSE_LMA_CURRENT_PURE_RMS] = ((values[9] >= 0.0f) && (pure_weight > 0.0f)) ?
 			sqrtf(__fdiv_rn(values[9], pure_weight)) : nanf("");
+	result[POSE_LMA_CANDIDATE_RMS] = nanf("");
+	result[POSE_LMA_CANDIDATE_PURE_RMS] = nanf("");
+}
+
+extern "C" __global__ void pose_lma_reduce_rms(
+		int           num_partials,
+		float         pure_weight,
+		const float * partials,
+		float       * result)
+{
+	if ((blockIdx.x != 0) || (threadIdx.x != 0)) return;
+	float pure_l2 = 0.0f;
+	float full_l2 = 0.0f;
+	for (int partial = 0; partial < num_partials; partial++) {
+		const int base = partial * POSE_LMA_PARTIAL_VALUES;
+		pure_l2 = pose_lma_add(pure_l2, partials[base + 9]);
+		full_l2 = pose_lma_add(full_l2, partials[base + 10]);
+	}
+	result[POSE_LMA_CANDIDATE_RMS] = (full_l2 >= 0.0f) ? sqrtf(full_l2) : nanf("");
+	result[POSE_LMA_CANDIDATE_PURE_RMS] =
+			((pure_l2 >= 0.0f) && (pure_weight > 0.0f)) ?
+			sqrtf(__fdiv_rn(pure_l2, pure_weight)) : nanf("");
 }
 
 extern "C" __global__ void lma_normal_products(

src/tp_lma.h

@@ -31,8 +31,12 @@
 #define POSE_LMA_RESULT      19
 #define POSE_LMA_UNIQUE_PRODUCTS  9
 #define POSE_LMA_PARTIAL_VALUES  11
-#define POSE_LMA_RESIDENT_RESULT 21
+#define POSE_LMA_RESIDENT_RESULT 23
 #define POSE_LMA_REDUCE_THREADS 128
+#define POSE_LMA_CURRENT_RMS     19
+#define POSE_LMA_CURRENT_PURE_RMS 20
+#define POSE_LMA_CANDIDATE_RMS   21
+#define POSE_LMA_CANDIDATE_PURE_RMS 22
 
 typedef struct {
 	int   width;
@@ -100,6 +104,14 @@ extern "C" __global__ void pose_lma_reduce_step(
 		const float * vector,
 		float       * result);        // [POSE_LMA_RESIDENT_RESULT]
 
+// Reduce a separately projected candidate pose to RMS only. Results append to
+// the compact resident step record without changing its candidate/normal terms.
+extern "C" __global__ void pose_lma_reduce_rms(
+		int           num_partials,
+		float         pure_weight,
+		const float * partials,
+		float       * result);        // candidate RMS fields in resident result
+
 extern "C" __global__ void lma_normal_products(
 		int            num_params,
 		int            num_values,