sfmTransform: Add option to lineup camera motion with object/lidar given an external camera pose

src/aliceVision/mesh/CMakeLists.txt

@@ -12,7 +12,7 @@ set(mesh_files_headers
   UVAtlas.hpp
   ModQuadricMetricT.hpp
   QuadricMetricT.hpp
-
+  MeshIntersection.hpp
 )
 
 # Sources
@@ -27,6 +27,7 @@ set(mesh_files_sources
   Texturing.cpp
   UVAtlas.cpp
   ModQuadricMetricT.cpp
+  MeshIntersection.cpp
 )
 
 alicevision_add_library(aliceVision_mesh

src/aliceVision/mesh/MeshIntersection.cpp

@@ -0,0 +1,62 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2024 AliceVision contributors.
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+
+#include "MeshIntersection.hpp"
+
+#include <geogram/basic/geometry.h>
+#include <geogram/mesh/mesh_io.h>
+#include <geogram/mesh/mesh_geometry.h>
+
+namespace aliceVision {
+namespace mesh {
+
+
+bool MeshIntersection::initialize(const std::string & pathToModel)
+{
+    GEO::initialize();
+    if (!GEO::mesh_load(pathToModel, _mesh))
+    {
+        ALICEVISION_LOG_ERROR("Impossible to load object file at " << pathToModel);
+        return false;
+    }
+
+    _aabb.initialize(_mesh);
+
+    return true;
+}
+
+bool MeshIntersection::peek(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords)
+{
+    const Vec3 posCamera = _pose.center();
+    const Vec3 wdir = intrinsic.backproject(imageCoords, true, _pose, 1.0);
+    const Vec3 dir = (wdir - posCamera).normalized();
+
+    //Create geogram ray from alicevision ray
+    GEO::Ray ray;
+    ray.origin.x = posCamera.x();
+    ray.origin.y = -posCamera.y();
+    ray.origin.z = -posCamera.z();
+    ray.direction.x = dir.x();
+    ray.direction.y = -dir.y();
+    ray.direction.z = -dir.z();
+
+    GEO::MeshFacetsAABB::Intersection intersection;
+    if (!_aabb.ray_nearest_intersection(ray, intersection)) 
+    {
+        return false;
+    }
+
+    const GEO::vec3 p = ray.origin + intersection.t * ray.direction;
+
+    output.x() = p.x;
+    output.y() = -p.y;
+    output.z() = -p.z;
+
+    return true;
+}
+
+}
+}

src/aliceVision/mesh/MeshIntersection.hpp

@@ -0,0 +1,49 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2024 AliceVision contributors.
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+
+#include <aliceVision/sfmData/SfMData.hpp>
+
+#include <geogram/mesh/mesh.h>
+#include <geogram/mesh/mesh_AABB.h>
+
+namespace aliceVision {
+namespace mesh {
+
+class MeshIntersection
+{
+public:
+    /**
+     * @brief initialize object. to be called before any other methodd
+     * @param pathToModel path to obj file to use as mesh
+    */
+    bool initialize(const std::string & pathToModel);
+
+    /**
+     * @brief Update pose to use for peeking
+     * @param pose transformation to use (in aliceVision standard form)
+    */
+    void setPose(const geometry::Pose3 & pose)
+    {
+        _pose = pose;
+    }
+
+    /**
+     * @brief peek a point on the mesh given a input camera observation
+     * @param output the output measured point
+     * @param intrinsic the camera intrinsics to use for ray computation
+     * @param imageCoords the camera observation we want to use to estimate its 'depth'
+     * @return true if the ray intersect the mesh.
+    */
+    bool peek(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords);
+
+private:
+    GEO::Mesh _mesh;
+    GEO::MeshFacetsAABB _aabb;
+    geometry::Pose3 _pose;
+};
+
+}
+}

src/aliceVision/sfm/CMakeLists.txt

@@ -43,6 +43,7 @@ set(sfm_files_headers
   pipeline/regionsIO.hpp
   utils/alignment.hpp
   utils/statistics.hpp
+  utils/preprocess.hpp
   utils/syntheticScene.hpp
   LocalBundleAdjustmentGraph.hpp
   FrustumFilter.hpp
@@ -80,6 +81,7 @@ set(sfm_files_sources
   pipeline/expanding/LbaPolicyConnexity.cpp
   pipeline/regionsIO.cpp
   utils/alignment.cpp
+  utils/preprocess.cpp
   utils/statistics.cpp
   utils/syntheticScene.cpp
   LocalBundleAdjustmentGraph.cpp

src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp

@@ -13,6 +13,7 @@
 #include <aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.hpp>
 #include <aliceVision/sfm/sfmFilters.hpp>
 #include <aliceVision/sfm/sfmStatistics.hpp>
+#include <aliceVision/sfm/utils/preprocess.hpp>
 
 #include <aliceVision/feature/FeaturesPerView.hpp>
 #include <aliceVision/graph/connectedComponent.hpp>
@@ -187,7 +188,7 @@ bool ReconstructionEngine_sequentialSfM::process()
         // If we have already reconstructed landmarks, we need to recognize the corresponding tracks
         // and update the landmarkIds accordingly.
         // Note: each landmark has a corresponding track with the same id (landmarkId == trackId).
-        remapLandmarkIdsToTrackIds();
+        remapLandmarkIdsToTrackIds(_sfmData, _map_tracks);
 
         if (_params.useLocalBundleAdjustment)
         {
@@ -402,62 +403,6 @@ void ReconstructionEngine_sequentialSfM::registerChanges(std::set<IndexT>& linke
     }
 }
 
-void ReconstructionEngine_sequentialSfM::remapLandmarkIdsToTrackIds()
-{
-    using namespace track;
-
-    // get unmap landmarks
-    Landmarks landmarks;
-
-    // clear sfmData structure and store them locally
-    std::swap(landmarks, _sfmData.getLandmarks());
-
-    // builds landmarks temporary comparison structure
-    // ObsKey <ViewId, FeatId, decType>
-    // ObsToLandmark <ObsKey, LandmarkId>
-    using ObsKey = std::tuple<IndexT, IndexT, feature::EImageDescriberType>;
-    using ObsToLandmark = std::map<ObsKey, IndexT>;
-
-    ObsToLandmark obsToLandmark;
-
-    ALICEVISION_LOG_DEBUG("Builds landmarks temporary comparison structure");
-
-    for (const auto& landmarkPair : landmarks)
-    {
-        const IndexT landmarkId = landmarkPair.first;
-        const IndexT firstViewId = landmarkPair.second.getObservations().begin()->first;
-        const IndexT firstFeatureId = landmarkPair.second.getObservations().begin()->second.getFeatureId();
-        const feature::EImageDescriberType descType = landmarkPair.second.descType;
-
-        obsToLandmark.emplace(ObsKey(firstViewId, firstFeatureId, descType), landmarkId);
-    }
-
-    ALICEVISION_LOG_DEBUG("Find corresponding landmark id per track id");
-
-    // find corresponding landmark id per track id
-    for (const auto& trackPair : _map_tracks)
-    {
-        const IndexT trackId = trackPair.first;
-        const Track& track = trackPair.second;
-
-        for (const auto& featView : track.featPerView)
-        {
-            const ObsToLandmark::const_iterator it = obsToLandmark.find(ObsKey(featView.first, featView.second.featureId, track.descType));
-
-            if (it != obsToLandmark.end())
-            {
-                // re-insert the landmark with the new id
-                _sfmData.getLandmarks().emplace(trackId, landmarks.find(it->second)->second);
-                break;  // one landmark per track
-            }
-        }
-    }
-
-    ALICEVISION_LOG_INFO("Landmark ids to track ids remapping: " << std::endl
-                                                                 << "\t- # tracks: " << _map_tracks.size() << std::endl
-                                                                 << "\t- # input landmarks: " << landmarks.size() << std::endl
-                                                                 << "\t- # output landmarks: " << _sfmData.getLandmarks().size());
-}
 
 double ReconstructionEngine_sequentialSfM::incrementalReconstruction()
 {

src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.hpp

@@ -147,11 +147,6 @@ class ReconstructionEngine_sequentialSfM : public ReconstructionEngine
      */
     void createInitialReconstruction(const std::vector<Pair>& initialImagePairCandidates);
 
-    /**
-     * @brief If we have already reconstructed landmarks in a previous reconstruction,
-     * we need to recognize the corresponding tracks and update the landmarkIds accordingly.
-     */
-    void remapLandmarkIdsToTrackIds();
 
     /**
      * @brief Loop of reconstruction updates

src/aliceVision/sfm/utils/alignment.cpp

@@ -610,6 +610,121 @@ bool computeSimilarityFromCommonLandmarks(const sfmData::SfMData& sfmDataA,
     return true;
 }
 
+bool computeSimilarityFromPairs(const std::vector<Vec3> & ptsA,
+                                const std::vector<Vec3> & ptsB,
+                                std::mt19937& randomNumberGenerator,
+                                double* out_S,
+                                Mat3* out_R,
+                                Vec3* out_t)
+{
+    // Move input point in appropriate container
+    Mat xA(3, ptsA.size());
+    Mat xB(3, ptsB.size());
+
+    int count = 0;
+    for (auto & p : ptsA)
+    {
+        xA.col(count) = p;
+        count++;
+    }
+
+    count = 0;
+    for (auto & p : ptsB)
+    {
+        xB.col(count) = p;
+        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 " << ptsA.size() << " common points and " << inliers.size()
+                                       << " were used to compute the similarity transform.");
+
+    *out_S = S;
+    *out_R = R;
+    *out_t = t;
+
+    return true;
+}
+
+bool computeNewCoordinateSystemFromPairs(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))
+    {
+        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

@@ -127,6 +127,13 @@ bool computeSimilarityFromCommonLandmarks(const sfmData::SfMData& sfmDataA,
                                         Mat3* out_R,
                                         Vec3* out_t);
 
+bool computeSimilarityFromPairs(const std::vector<Vec3> & ptsA,
+                                const std::vector<Vec3> & ptsB,
+                                std::mt19937& randomNumberGenerator,
+                                double* out_S,
+                                Mat3* out_R,
+                                Vec3* out_t);
+
 /**
  * @brief Apply a transformation the given SfMData
  *