feat(kpm): report every matched page, not only the best one

lib/SRC/KPM/FreakMatcher/facade/visual_database_facade.cpp

@@ -150,7 +150,11 @@ namespace vision {
     const matches_t& VisualDatabaseFacade::inliers() const{
         return mVisualDbImpl->mVdb->inliers();
     }
-
+
+    const image_matches_t& VisualDatabaseFacade::matches() const{
+        return mVisualDbImpl->mVdb->matches();
+    }
+
     int VisualDatabaseFacade::getWidth(int image_id) const{
         return mVisualDbImpl->mVdb->keyframe(image_id)->width();
     }

lib/SRC/KPM/FreakMatcher/facade/visual_database_facade.h

@@ -91,7 +91,9 @@ namespace vision {
         const std::vector<unsigned char>& getQueryDescriptors() const;
 
         const matches_t& inliers() const;
-
+
+        const image_matches_t& matches() const;
+
     private:
         std::unique_ptr<VisualDatabaseImpl> mVisualDbImpl;
     }; // VisualDatabaseFacade

lib/SRC/KPM/FreakMatcher/matchers/matcher_types.h

@@ -47,5 +47,16 @@ namespace vision {
     }; // match_t
 
     typedef std::vector<match_t> matches_t;
-
+
+    /**
+     * One reference image that survived geometric verification against the query.
+     */
+    struct image_match_t {
+        int id;             // reference image id: the db_id passed to addImage()
+        matches_t inliers;  // correspondences consistent with `geometry`
+        float geometry[9];  // homography, row-major
+    }; // image_match_t
+
+    typedef std::vector<image_match_t> image_matches_t;
+
 } // vision

lib/SRC/KPM/FreakMatcher/matchers/visual_database-inline.h

@@ -193,7 +193,8 @@ namespace vision {
     bool VisualDatabase<FEATURE_EXTRACTOR, STORE, MATCHER>::query(const keyframe_t* query_keyframe) {
         mMatchedInliers.clear();
         mMatchedId = -1;
-
+        mMatches.clear();
+
         const std::vector<FeaturePoint>& query_points = query_keyframe->store().points();
 
         // Loop over all the images in the database
@@ -337,10 +338,20 @@ namespace vision {
             }
 
             //std::cout<<"inliers-"<<inliers.size()<<std::endl;
-            if(inliers.size() >= mMinNumInliers && inliers.size() > mMatchedInliers.size()) {
-                CopyVector9(mMatchedGeometry, H);
-                mMatchedInliers.swap(inliers);
-                mMatchedId = it->first;
+            if(inliers.size() >= mMinNumInliers) {
+                // Keep every image that passes, not only the best (#635): with
+                // several markers in view, each one has its own match.
+                image_match_t match;
+                match.id = it->first;
+                match.inliers = inliers;
+                CopyVector9(match.geometry, H);
+                mMatches.push_back(match);
+
+                if(inliers.size() > mMatchedInliers.size()) {
+                    CopyVector9(mMatchedGeometry, H);
+                    mMatchedInliers.swap(inliers);
+                    mMatchedId = it->first;
+                }
             }
         }
 

lib/SRC/KPM/FreakMatcher/matchers/visual_database.h

@@ -159,7 +159,14 @@ namespace vision {
          * @return Matched geometry matrix
          */
         const float* matchedGeometry() const { return mMatchedGeometry; }
-
+
+        /**
+         * @return Every reference image that passed the inlier tests in the last
+         * query(), in database order. matchedId()/inliers()/matchedGeometry()
+         * still describe the single best of these.
+         */
+        const image_matches_t& matches() const { return mMatches; }
+
         /**
          * Get the detector.
          */
@@ -183,7 +190,8 @@ namespace vision {
         matches_t mMatchedInliers;
         id_t mMatchedId;
         float mMatchedGeometry[9];
-
+        image_matches_t mMatches;
+
         keyframe_ptr_t mQueryKeyframe;
 
         // Map of keyframe

lib/SRC/KPM/kpmMatching.cpp

@@ -39,6 +39,8 @@
 #include <string>
 #include <sstream>
 #include <algorithm>
+#include <map>
+#include <vector>
 
 #include <KPM/kpm.h>
 #include "kpmPrivate.h"
@@ -293,6 +295,7 @@ int kpmSetRefDataSet( KpmHandle *kpmHandle, KpmRefDataSet *refDataSet )
                 }
                 ARLOGi("page %d, image num %d, points - %d\n", k, m, points.size());
                 kpmHandle->pageIDs[db_id] = kpmHandle->refDataSet.pageInfo[k].pageNo;
+                kpmHandle->pageIndices[db_id] = k;
                 ARLOGd("kpmSetRefDataSet: db_id=%d -> pageNo=%d (page index %d, image %d, %d points)\n", db_id, kpmHandle->refDataSet.pageInfo[k].pageNo, k, m, (int)points.size());
                 kpmHandle->freakMatcher->addFreakFeaturesAndDescriptors(points,descriptors,points_3d,kpmHandle->refDataSet.pageInfo[k].imageInfo[m].width,kpmHandle->refDataSet.pageInfo[k].imageInfo[m].height,db_id++);
             }
@@ -638,50 +641,47 @@ for (int pageLoop = 0; pageLoop < kpmHandle->resultNum; pageLoop++) {
     kpmHandle->result[pageLoop].camPoseF = -1;
 }
 
-const vision::matches_t& matches = kpmHandle->freakMatcher->inliers();
-int matched_image_id = kpmHandle->freakMatcher->matchedId();
-ARLOGd("kpmMatching: matchedId=%d (resultNum=%d, inliers=%d)\n", matched_image_id, kpmHandle->resultNum, (int)matches.size());
-if (matched_image_id >= 0) {
-    int matchedPageNo = kpmHandle->pageIDs[matched_image_id];
-    ARLOGd("kpmMatching: matchedId=%d maps to pageNo=%d\n", matched_image_id, matchedPageNo);
-
-    if( !kpmHandle->result[matchedPageNo].skipF ) {
+// Every reference image that passed the matcher's tests (#635). Each page was
+// registered as several images — one per scale, see kpmSetRefDataSet — so
+// several matches can belong to one page.
+//
+// result[] is indexed by the page's position in refDataSet.pageInfo, as
+// kpmSetMatchingSkipPage() indexes it, NOT by the page's external number:
+// page numbers are labels (kpmChangePageNoOfRefDataSet can set any value).
+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);
+}
+ARLOGd("kpmMatching: %d image match(es) across %d page(s)\n", (int)imageMatches.size(), (int)candidatesPerPage.size());
+
+for (auto& entry : candidatesPerPage) {
+    const int pageIndex = entry.first;
+    KpmResult& result = kpmHandle->result[pageIndex];
+    if (result.skipF) continue;
+
+    // Try the page's best-supported image first. If its pose cannot be fitted,
+    // fall back to the page's other verified images rather than dropping the page.
+    std::vector<const vision::image_match_t*>& candidates = entry.second;
+    std::stable_sort(candidates.begin(), candidates.end(),
+                     [](const vision::image_match_t* a, const vision::image_match_t* b) {
+                         return a->inliers.size() > b->inliers.size();
+                     });
+    for (const vision::image_match_t* imageMatch : candidates) {
         ret = kpmUtilGetPose_binary(kpmHandle->cparamLT,
-                                    matches ,
-                                    kpmHandle->freakMatcher->get3DFeaturePoints(matched_image_id),
+                                    imageMatch->inliers,
+                                    kpmHandle->freakMatcher->get3DFeaturePoints(imageMatch->id),
                                     kpmHandle->freakMatcher->getQueryFeaturePoints(),
-                                    kpmHandle->result[matchedPageNo].camPose,
-                                    &(kpmHandle->result[matchedPageNo].error) );
-
-        if (ret == 0) {
-            kpmHandle->result[matchedPageNo].camPoseF = 0;
-            kpmHandle->result[matchedPageNo].inlierNum = (int)matches.size();
-            kpmHandle->result[matchedPageNo].pageNo = matchedPageNo;
-            ARLOGi("Page[%d]  pre:%3d, aft:%3d, error = %f\n", matchedPageNo, (int)matches.size(), (int)matches.size(), kpmHandle->result[matchedPageNo].error);
-        }
+                                    result.camPose,
+                                    &(result.error));
+        if (ret != 0) continue;
+        result.camPoseF = 0;
+        result.inlierNum = (int)imageMatch->inliers.size();
+        result.pageNo = kpmHandle->refDataSet.pageInfo[pageIndex].pageNo;
+        ARLOGi("Page[%d]  pre:%3d, aft:%3d, error = %f\n", result.pageNo, (int)imageMatch->inliers.size(), (int)imageMatch->inliers.size(), result.error);
+        break;
     }
 }
-/*
-        for (int pageLoop = 0; pageLoop < kpmHandle->resultNum; pageLoop++) {
-
-            kpmHandle->result[pageLoop].pageNo = kpmHandle->refDataSet.pageInfo[pageLoop].pageNo;
-            kpmHandle->result[pageLoop].camPoseF = -1;
-            if( kpmHandle->result[pageLoop].skipF ) continue;
-
-
-            const vision::matches_t& matches = kpmHandle->freakMatcher->inliers();
-            int matched_image_id = kpmHandle->freakMatcher->matchedId();
-            if (matched_image_id < 0) continue;
-
-            //ARLOGi("Pose (freak) - %s",arrayToString2(kpmHandle->result[pageLoop].camPose).c_str());
-            if( ret == 0 ) {
-                kpmHandle->result[pageLoop].camPoseF = 0;
-                kpmHandle->result[pageLoop].inlierNum = (int)matches.size();
-                kpmHandle->result[pageLoop].pageNo = kpmHandle->pageIDs[matched_image_id];
-                ARLOGi("Page[%d]  pre:%3d, aft:%3d, error = %f\n", pageLoop, (int)matches.size(), (int)matches.size(), kpmHandle->result[pageLoop].error);
-            }
-        }
-        */
 #endif
 #if !BINARY_FEATURE
         free(featureVector.sf);

lib/SRC/KPM/kpmPrivate.h

@@ -88,6 +88,7 @@ struct _KpmHandle {
     KpmResult                *result;
     int                       resultNum;
     int                       pageIDs[DB_IMAGE_MAX];
+    int                       pageIndices[DB_IMAGE_MAX]; // position in refDataSet.pageInfo (and result[]) of each db_id's page
 };
 
 #endif // !__kpmPrivate_h__