CODEX: Keep pose LMA acceptance state resident

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=23 };
+enum { NUM_PARAMS=3, NUM_COMPONENTS=2, PRODUCTS=12, RESULT=25 };
 
 extern "C" {
   void* tp_create_module(const char*, const char*);
@@ -21,7 +21,7 @@ extern "C" {
       const float*,const float*,const unsigned char*,int,int,
       float*,float*,unsigned char*);
   int tp_proc_exec_pose_lma_resident_step(
-      void*,float,const float*,const float*,const float*,const float*,float,
+      void*,float,const float*,const float*,const float*,const float*,float,float,
       int,int,float*,float*,float*);
   void tp_proc_destroy(void*);
   void tp_destroy_module(void*);
@@ -50,7 +50,8 @@ int main(int argc,char** argv)
   const float radial[7]={0,0,0,0,0,0,0};
   std::vector<float> rbr(rbrLen,1.0f),ers(ersLen,0.0f);
   for(int line=0;line<height;line++) ers[(size_t)line*14+6]=1.0f;
-  const float pose[12]={0,0,0, 0,0,0, 0,0,0, 0,0,0};
+  const float vector[NUM_PARAMS]={0.0023f,-0.0017f,0.00091f};
+  const float pose[12]={0,0,0, 0,0,0, 0,0,0, vector[0],vector[1],vector[2]};
   const float centers[3*numTiles]={20,10,5, 40,30,0, 12,16,3};
   const unsigned char selection[numTiles]={1,1,0};
   float rawFx[2*numTiles],rawJt[NUM_PARAMS*2*numTiles]; unsigned char valid[numTiles];
@@ -61,8 +62,7 @@ int main(int argc,char** argv)
       pose,centers,selection,numTiles,1,rawFx,rawJt,valid);
   if(rc){ std::fprintf(stderr,"pose jacobian rc=%d: %s\n",rc,tp_last_error()); bad++; }
 
-  const float lambda=0.001f,pureWeight=0.98f;
-  const float vector[NUM_PARAMS]={0.0023f,-0.0017f,0.00091f};
+  const float lambda=0.001f,pureWeight=0.98f,rmsDiff=0.001f;
   float weights[numValues]={0},y[numValues]={0},eigen[4*numTiles]={0};
   for(int tile=0;tile<2;tile++){
     const int vi=2*tile,ei=4*tile;
@@ -78,7 +78,7 @@ int main(int argc,char** argv)
   }
   float packed[RESULT],preparedJt[NUM_PARAMS*numValues],preparedYmfx[numValues];
   rc=tp_proc_exec_pose_lma_resident_step(proc,lambda,weights,y,eigen,vector,
-      pureWeight,numTiles,1,packed,preparedJt,preparedYmfx);
+      pureWeight,rmsDiff,numTiles,1,packed,preparedJt,preparedYmfx);
   if(rc){ std::fprintf(stderr,"resident step rc=%d: %s\n",rc,tp_last_error()); bad++; }
 
   float expectedJt[NUM_PARAMS*numValues]={0},expectedYmfx[numValues]={0};
@@ -117,6 +117,30 @@ int main(int argc,char** argv)
       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++;
     }
+    const bool expectedAccepted=packed[0]!=0.0f&&packed[21]<packed[19];
+    const bool expectedStop=expectedAccepted&&packed[21]>=packed[19]*(1.0f-rmsDiff);
+    if((packed[23]!=float(expectedAccepted))||(packed[24]!=float(expectedStop))){
+      std::printf("decision mismatch accepted=%g/%d stop=%g/%d\n",
+          packed[23],expectedAccepted,packed[24],expectedStop); bad++;
+    }
+
+    // An accepted step swaps the candidate raw pose into the current pointers.
+    // Re-running the resident preparation at that vector must therefore start
+    // at the previous candidate RMS without a public pose re-launch.
+    if(expectedAccepted){
+      float nextVector[NUM_PARAMS]={packed[16],packed[17],packed[18]};
+      float nextPacked[RESULT];
+      const int nextRc=tp_proc_exec_pose_lma_resident_step(proc,lambda,weights,y,eigen,nextVector,
+          pureWeight,rmsDiff,numTiles,0,nextPacked,nullptr,nullptr);
+      if(nextRc||std::fabs(nextPacked[19]-packed[21])>2.0e-6f||
+         std::fabs(nextPacked[20]-packed[22])>2.0e-6f){
+        std::printf("accepted-state mismatch rc=%d current=%g/%g candidate-before=%g/%g\n",
+            nextRc,nextPacked[19],nextPacked[20],packed[21],packed[22]); bad++;
+      }
+    }else{
+      std::printf("test setup did not produce an accepted resident step current=%g candidate=%g\n",
+          packed[19],packed[21]); bad++;
+    }
 
     // Re-project the returned candidate through the public pose entry point and
     // independently reproduce its float RMS. The resident call itself used
@@ -163,8 +187,42 @@ int main(int argc,char** argv)
             maxRms,expectedFull,expectedPure,packed[21],packed[22]); bad++;
       }
     }
+
+    // Restore a deliberately inconsistent raw/vector setup whose candidate is
+    // known to worsen. A second resident call must see the same current RMS,
+    // proving that rejection retained the fallback pointers.
+    const float zeroPose[12]={0};
+    float rejectFx[2*numTiles],rejectJt[NUM_PARAMS*2*numTiles];
+    unsigned char rejectValid[numTiles];
+    const int rejectPoseRc=tp_proc_exec_pose_fx_jacobian(
+        proc,meta,radial,rbr.data(),rbrLen,ers.data(),ersLen,
+        meta,radial,rbr.data(),rbrLen,ers.data(),ersLen,
+        zeroPose,centers,selection,numTiles,1,rejectFx,rejectJt,rejectValid);
+    if(rejectPoseRc){
+      std::fprintf(stderr,"reject pose rc=%d: %s\n",rejectPoseRc,tp_last_error()); bad++;
+    }else{
+      float rejectY[numValues]; std::memcpy(rejectY,y,sizeof(rejectY));
+      for(int tile=0;tile<2;tile++){
+        const int vi=2*tile;
+        rejectY[vi]=rejectFx[vi]+0.031f-0.004f*tile;
+        rejectY[vi+1]=rejectFx[vi+1]-0.022f+0.003f*tile;
+      }
+      float rejectPacked[RESULT]={0},retainedPacked[RESULT]={0};
+      const int rejectRc=tp_proc_exec_pose_lma_resident_step(proc,lambda,weights,rejectY,eigen,vector,
+          pureWeight,rmsDiff,numTiles,0,rejectPacked,nullptr,nullptr);
+      const int retainedRc=rejectRc?rejectRc:tp_proc_exec_pose_lma_resident_step(
+          proc,lambda,weights,rejectY,eigen,vector,pureWeight,rmsDiff,numTiles,0,
+          retainedPacked,nullptr,nullptr);
+      if(rejectRc||retainedRc||(rejectPacked[23]!=0.0f)||
+         std::fabs(retainedPacked[19]-rejectPacked[19])>2.0e-6f||
+         std::fabs(retainedPacked[20]-rejectPacked[20])>2.0e-6f){
+        std::printf("rejected-state mismatch rc=%d/%d accepted=%g current=%g/%g retained=%g/%g\n",
+            rejectRc,retainedRc,rejectPacked[23],rejectPacked[19],rejectPacked[20],
+            retainedPacked[19],retainedPacked[20]); bad++;
+      }
+    }
   }
-  std::printf("NVRTC resident pose LMA: reused raw fx/J + prepared oracle + compact step -> %s\n",
+  std::printf("NVRTC resident pose LMA: candidate RMS + accept/reject state transition -> %s\n",
       bad?"FAIL":"PASS");
   tp_proc_destroy(proc); tp_destroy_module(module);
   return bad?EXIT_FAILURE:EXIT_SUCCESS;

jna/tp_jna.cpp

@@ -1385,13 +1385,14 @@ int tp_proc_exec_pose_lma_resident_step(TpProc* p,
                                         const float* eigen,
                                         const float* vector,
                                         float pure_weight,
+                                        float rms_diff,
                                         int num_tiles,
                                         int capture_prepared,
                                         float* result,
                                         float* prepared_jt,
                                         float* prepared_ymfx){
   if(!p||!weights||!y||!eigen||!vector||!result||(num_tiles<=0)||
-     !std::isfinite(pure_weight)||!(pure_weight>0.0f)||
+     !std::isfinite(pure_weight)||!(pure_weight>0.0f)||!std::isfinite(rms_diff)||
      (capture_prepared&&(!prepared_jt||!prepared_ymfx))){
     seterr("exec_pose_lma_resident_step: bad args p=%p nt=%d capture=%d",
            (void*)p,num_tiles,capture_prepared); return -1; }
@@ -1485,12 +1486,21 @@ int tp_proc_exec_pose_lma_resident_step(TpProc* p,
                 &p->gpu_pose_lma_candidate_ymfx,&p->gpu_pose_lma_candidate_partials };
   if(launch1(prepare,num_partials,1,1,POSE_LMA_REDUCE_THREADS,1,1,cpa,
              "pose_lma_prepare_products(candidate)")) return -12;
-  void* cr[]={ &num_partials,&pure_weight,&p->gpu_pose_lma_candidate_partials,
+  void* cr[]={ &num_partials,&pure_weight,&rms_diff,&p->gpu_pose_lma_candidate_partials,
                &p->gpu_pose_lma_result };
   if(launch1(reduce_rms,1,1,1,1,1,1,cr,"pose_lma_reduce_rms")) return -13;
   if(cudaMemcpy(result,p->gpu_pose_lma_result,
                 POSE_LMA_RESIDENT_RESULT*sizeof(float),cudaMemcpyDeviceToHost)!=cudaSuccess){
     seterr("exec_pose_lma_resident_step: result DtoH failed"); return -14; }
+  // The CUDA comparison is the state-transition oracle for this rung. Swapping
+  // pointers makes the accepted candidate the next current raw pose without a
+  // tile-array copy; rejection leaves the current/fallback buffers untouched.
+  if(result[POSE_LMA_ACCEPTED]!=0.0f){
+    std::swap(p->gpu_pose_vectors,p->gpu_pose_candidate_vectors);
+    std::swap(p->gpu_pose_fx,p->gpu_pose_candidate_fx);
+    std::swap(p->gpu_pose_jt,p->gpu_pose_candidate_jt);
+    std::swap(p->gpu_pose_valid,p->gpu_pose_candidate_valid);
+  }
   if(capture_prepared &&
      (cudaMemcpy(prepared_jt,p->gpu_pose_lma_jt,
                  (size_t)POSE_NUM_PARAMS*num_values*sizeof(float),cudaMemcpyDeviceToHost)!=cudaSuccess ||

src/tests/test_pose_lma_step.cu

@@ -253,7 +253,7 @@ 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,
+	pose_lma_reduce_rms<<<1, 1>>>(numPartials, pureWeight, 0.001f,
 			gpuPartials.dev(), gpuResidentResult.dev());
 	checkCudaErrors(cudaGetLastError()); checkCudaErrors(cudaDeviceSynchronize());
 	const std::vector<float> gotJt = gpuPreparedJt.download();
@@ -277,7 +277,9 @@ int main(int argc, char ** argv)
 	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);
+			(maxCandidate <= 2.0e-5f) && (maxRms <= 2.0e-6f) &&
+			(gotResident[POSE_LMA_ACCEPTED] == 0.0f) &&
+			(gotResident[POSE_LMA_STOP] == 0.0f);
 	std::printf("pose_lma resident 5120/150: prepared bit mismatches=%d, max products=%g, delta=%g, candidate=%g, RMS=%g -> %s\n",
 			preparedBad, maxProducts, maxDelta, maxCandidate, maxRms,
 			residentOk ? "PASS" : "FAIL");

src/tp_lma.cu

@@ -728,11 +728,14 @@ extern "C" __global__ void pose_lma_reduce_step(
 			sqrtf(__fdiv_rn(values[9], pure_weight)) : nanf("");
 	result[POSE_LMA_CANDIDATE_RMS] = nanf("");
 	result[POSE_LMA_CANDIDATE_PURE_RMS] = nanf("");
+	result[POSE_LMA_ACCEPTED] = 0.0f;
+	result[POSE_LMA_STOP] = 0.0f;
 }
 
 extern "C" __global__ void pose_lma_reduce_rms(
 		int           num_partials,
 		float         pure_weight,
+		float         rms_diff,
 		const float * partials,
 		float       * result)
 {
@@ -748,6 +751,15 @@ extern "C" __global__ void pose_lma_reduce_rms(
 	result[POSE_LMA_CANDIDATE_PURE_RMS] =
 			((pure_l2 >= 0.0f) && (pure_weight > 0.0f)) ?
 			sqrtf(__fdiv_rn(pure_l2, pure_weight)) : nanf("");
+	const float current_rms = result[POSE_LMA_CURRENT_RMS];
+	const float candidate_rms = result[POSE_LMA_CANDIDATE_RMS];
+	const bool accepted = (result[0] != 0.0f) && isfinite(current_rms) &&
+			isfinite(candidate_rms) && (candidate_rms < current_rms);
+	const bool stop = accepted &&
+			(candidate_rms >= pose_lma_mul(current_rms,
+					pose_lma_sub(1.0f, rms_diff)));
+	result[POSE_LMA_ACCEPTED] = accepted ? 1.0f : 0.0f;
+	result[POSE_LMA_STOP] = stop ? 1.0f : 0.0f;
 }
 
 extern "C" __global__ void lma_normal_products(

src/tp_lma.h

@@ -31,12 +31,14 @@
 #define POSE_LMA_RESULT      19
 #define POSE_LMA_UNIQUE_PRODUCTS  9
 #define POSE_LMA_PARTIAL_VALUES  11
-#define POSE_LMA_RESIDENT_RESULT 23
+#define POSE_LMA_RESIDENT_RESULT 25
 #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
+#define POSE_LMA_ACCEPTED        23
+#define POSE_LMA_STOP            24
 
 typedef struct {
 	int   width;
@@ -109,6 +111,7 @@ extern "C" __global__ void pose_lma_reduce_step(
 extern "C" __global__ void pose_lma_reduce_rms(
 		int           num_partials,
 		float         pure_weight,
+		float         rms_diff,
 		const float * partials,
 		float       * result);        // candidate RMS fields in resident result