diff --git a/lib/SRC/KPM/kpmMatching.cpp b/lib/SRC/KPM/kpmMatching.cpp
index 4246ace..28ec29b 100644
--- a/lib/SRC/KPM/kpmMatching.cpp
+++ b/lib/SRC/KPM/kpmMatching.cpp
@@ -189,6 +189,24 @@ int kpmSetRefDataSet( KpmHandle *kpmHandle, KpmRefDataSet *refDataSet )
         ARLOGe("kpmSetRefDataSet(): refDataSet.\n");
         return -1;
     }
+#if BINARY_FEATURE
+    // pageIDs[] and pageIndices[] hold one entry per registered image (one per
+    // page per scale). Refuse a dataset that would not fit before changing any
+    // state, instead of writing past them.
+    {
+        // Check each count against the room left before adding it, so a corrupt
+        // or hostile dataset cannot overflow the running total past the check.
+        int imageTotal = 0;
+        for( i = 0; i < refDataSet->pageNum; i++ ) {
+            const int imageNum = refDataSet->pageInfo[i].imageNum;
+            if( imageNum < 0 || imageNum > DB_IMAGE_MAX - imageTotal ) {
+                ARLOGe("kpmSetRefDataSet(): page %d has %d reference images; the set exceeds DB_IMAGE_MAX (%d).\n", i, imageNum, DB_IMAGE_MAX);
+                return -1;
+            }
+            imageTotal += imageNum;
+        }
+    }
+#endif
 
     // Copy the refPoints into the kpmHandle's dataset.
     if( kpmHandle->refDataSet.refPoint != NULL ) {
@@ -269,6 +287,16 @@ int kpmSetRefDataSet( KpmHandle *kpmHandle, KpmRefDataSet *refDataSet )
         free(featureVector.sf);
     }
 #else
+    // Register the new set in a fresh matcher. Reusing the old one kept every
+    // keyframe it already held: an id the new set registers again is refused
+    // as a duplicate (the old keyframe stays) while its 3D points are replaced,
+    // so pose estimation paired old matches with new, shorter point lists. The
+    // matcher carries no configuration beyond construction (kpmCreateHandle),
+    // so a new instance is equivalent to the original.
+    delete kpmHandle->freakMatcher;
+    kpmHandle->freakMatcher = new vision::VisualDatabaseFacade;
+    kpmHandle->dbImageNum = 0;
+
     if (kpmHandle->refDataSet.num != 0) {
         featureVector.num = kpmHandle->refDataSet.num;
 
@@ -300,6 +328,7 @@ int kpmSetRefDataSet( KpmHandle *kpmHandle, KpmRefDataSet *refDataSet )
                 kpmHandle->freakMatcher->addFreakFeaturesAndDescriptors(points,descriptors,points_3d,kpmHandle->refDataSet.pageInfo[k].imageInfo[m].width,kpmHandle->refDataSet.pageInfo[k].imageInfo[m].height,db_id++);
             }
         }
+        kpmHandle->dbImageNum = db_id;
     }
 #endif
 
@@ -651,7 +680,19 @@ for (int pageLoop = 0; pageLoop < kpmHandle->resultNum; pageLoop++) {
 const vision::image_matches_t& imageMatches = kpmHandle->freakMatcher->matches();
 std::map<int, std::vector<const vision::image_match_t*> > candidatesPerPage;
 for (const vision::image_match_t& imageMatch : imageMatches) {
-    candidatesPerPage[kpmHandle->pageIndices[imageMatch.id]].push_back(&imageMatch);
+    // Invariant checks. kpmSetRefDataSet() registers each set in a fresh
+    // matcher, so every id is below dbImageNum and maps into result[]; should
+    // that ever break, log it rather than index the mapping arrays out of bounds.
+    if (imageMatch.id < 0 || imageMatch.id >= kpmHandle->dbImageNum) {
+        ARLOGe("kpmMatching: matcher image %d is outside the current set (%d images).\n", imageMatch.id, kpmHandle->dbImageNum);
+        continue;
+    }
+    const int pageIndex = kpmHandle->pageIndices[imageMatch.id];
+    if (pageIndex < 0 || pageIndex >= kpmHandle->resultNum) {
+        ARLOGe("kpmMatching: image %d maps to page position %d, outside 0..%d.\n", imageMatch.id, pageIndex, kpmHandle->resultNum - 1);
+        continue;
+    }
+    candidatesPerPage[pageIndex].push_back(&imageMatch);
 }
 ARLOGd("kpmMatching: %d image match(es) across %d page(s)\n", (int)imageMatches.size(), (int)candidatesPerPage.size());
 
diff --git a/lib/SRC/KPM/kpmPrivate.h b/lib/SRC/KPM/kpmPrivate.h
index bc87681..0b3a04a 100644
--- a/lib/SRC/KPM/kpmPrivate.h
+++ b/lib/SRC/KPM/kpmPrivate.h
@@ -89,6 +89,7 @@ struct _KpmHandle {
     int                       resultNum;
     int                       pageIDs[DB_IMAGE_MAX];
     int                       pageIndices[DB_IMAGE_MAX]; // position in refDataSet.pageInfo (and result[]) of each db_id's page
+    int                       dbImageNum;                // images registered by the last kpmSetRefDataSet(): ids 0..dbImageNum-1 are current
 };
 
 #endif // !__kpmPrivate_h__