Merge pull request #1730 from alicevision/dev/sfmMatching

Multiple shots: Align and merge multiple SfM from feature matches

Author: fabiencastan

src/aliceVision/sfm/utils/alignment.cpp

@@ -541,6 +541,75 @@ bool computeSimilarityFromCommonMarkers(const sfmData::SfMData& sfmDataA,
     return true;
 }
 
+bool computeSimilarityFromCommonLandmarks(const sfmData::SfMData& sfmDataA,
+                                        const sfmData::SfMData& sfmDataB,
+                                        std::mt19937& randomNumberGenerator,
+                                        double* out_S,
+                                        Mat3* out_R,
+                                        Vec3* out_t)
+{
+    //create map featureId, landmarkId
+    std::map<IndexT, IndexT> mapFeatureIdToLandmarkId;
+    for (const auto & plandmark : sfmDataA.getLandmarks())
+    {
+        for (const auto & pobs : plandmark.second.getObservations())
+        {
+            IndexT featureId = pobs.second.getFeatureId();
+            mapFeatureIdToLandmarkId[featureId] = plandmark.first;
+        }
+    }
+
+    std::map<IndexT, IndexT> mapLandmarkAtoLandmarkB;
+    for (const auto & plandmark : sfmDataB.getLandmarks())
+    {
+        for (const auto & pobs : plandmark.second.getObservations())
+        {
+            IndexT featureId = pobs.second.getFeatureId();
+            
+            auto found = mapFeatureIdToLandmarkId.find(featureId);
+            if (found == mapFeatureIdToLandmarkId.end())
+            {
+                continue;
+            }
+
+            mapLandmarkAtoLandmarkB[found->second] = plandmark.first;
+        }
+    }
+
+    // Move input point in appropriate container
+    Mat xA(3, mapLandmarkAtoLandmarkB.size());
+    Mat xB(3, mapLandmarkAtoLandmarkB.size());
+    
+    int count = 0;
+    for (auto & pair : mapLandmarkAtoLandmarkB)
+    {
+        xA.col(count) = sfmDataA.getLandmarks().at(pair.first).X;
+        xB.col(count) = sfmDataB.getLandmarks().at(pair.second).X;
+        count++;
+    }
+
+    // Compute rigid transformation p'i = S R pi + t
+    double S;
+    Vec3 t;
+    Mat3 R;
+    std::vector<std::size_t> inliers;
+
+    if (!aliceVision::geometry::ACRansac_FindRTS(xA, xB, randomNumberGenerator, S, t, R, inliers, true))
+    {
+        std::cout << "merde" << std::endl;
+        return false;
+    }
+
+    ALICEVISION_LOG_DEBUG("There are " << mapLandmarkAtoLandmarkB.size() << " common markers and " << inliers.size()
+                                       << " were used to compute the similarity transform.");
+
+    *out_S = S;
+    *out_R = R;
+    *out_t = t;
+
+    return true;
+}
+
 /**
  * Image orientation CCW
  */

src/aliceVision/sfm/utils/alignment.hpp

@@ -120,6 +120,13 @@ bool computeSimilarityFromCommonMarkers(const sfmData::SfMData& sfmDataA,
                                         Mat3* out_R,
                                         Vec3* out_t);
 
+bool computeSimilarityFromCommonLandmarks(const sfmData::SfMData& sfmDataA,
+                                        const sfmData::SfMData& sfmDataB,
+                                        std::mt19937& randomNumberGenerator,
+                                        double* out_S,
+                                        Mat3* out_R,
+                                        Vec3* out_t);
+
 /**
  * @brief Apply a transformation the given SfMData
  *

src/software/utils/CMakeLists.txt

@@ -211,6 +211,8 @@ if(ALICEVISION_BUILD_SFM)
               aliceVision_cmdline
               aliceVision_sfmData
               aliceVision_sfmDataIO
+              aliceVision_sfm
+              aliceVision_matching
               Boost::program_options
     )
 

src/software/utils/main_sfmAlignment.cpp

@@ -38,7 +38,8 @@ enum class EAlignmentMethod : unsigned char
     FROM_CAMERAS_POSEID,
     FROM_CAMERAS_FILEPATH,
     FROM_CAMERAS_METADATA,
-    FROM_MARKERS
+    FROM_MARKERS,
+    FROM_LANDMARKS,
 };
 
 /**
@@ -60,6 +61,8 @@ std::string EAlignmentMethod_enumToString(EAlignmentMethod alignmentMethod)
             return "from_cameras_metadata";
         case EAlignmentMethod::FROM_MARKERS:
             return "from_markers";
+        case EAlignmentMethod::FROM_LANDMARKS:
+            return "from_landmarks";
     }
     throw std::out_of_range("Invalid EAlignmentMethod enum");
 }
@@ -84,6 +87,8 @@ EAlignmentMethod EAlignmentMethod_stringToEnum(const std::string& alignmentMetho
         return EAlignmentMethod::FROM_CAMERAS_METADATA;
     if (method == "from_markers")
         return EAlignmentMethod::FROM_MARKERS;
+    if (method == "from_landmarks")
+        return EAlignmentMethod::FROM_LANDMARKS;
     throw std::out_of_range("Invalid SfM alignment method : " + alignmentMethod);
 }
 
@@ -130,6 +135,7 @@ int aliceVision_main(int argc, char** argv)
          "\t- from_cameras_poseid: Align cameras with same pose ID.\n"
          "\t- from_cameras_filepath: Align cameras with a filepath matching, using --fileMatchingPattern.\n"
          "\t- from_cameras_metadata: Align cameras with matching metadata, using --metadataMatchingList.\n"
+         "\t- from_landmarks: Alilgn using landmarks sharing features.\n"
          "\t- from_markers: Align from markers with the same ID.\n")
         ("fileMatchingPattern", po::value<std::string>(&fileMatchingPattern)->default_value(fileMatchingPattern),
          "Matching pattern for the from_cameras_filepath method.\n")
@@ -207,6 +213,11 @@ int aliceVision_main(int argc, char** argv)
             hasValidSimilarity = sfm::computeSimilarityFromCommonMarkers(sfmData, sfmDataInRef, randomNumberGenerator, &S, &R, &t);
             break;
         }
+        case EAlignmentMethod::FROM_LANDMARKS:
+        {
+            hasValidSimilarity = sfm::computeSimilarityFromCommonLandmarks(sfmData, sfmDataInRef, randomNumberGenerator, &S, &R, &t);
+            break;
+        }
     }
 
     if (!hasValidSimilarity)