diff --git a/src/aliceVision/mesh/CMakeLists.txt b/src/aliceVision/mesh/CMakeLists.txt index 32e6313510..a86545d21e 100644 --- a/src/aliceVision/mesh/CMakeLists.txt +++ b/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 diff --git a/src/aliceVision/mesh/MeshIntersection.cpp b/src/aliceVision/mesh/MeshIntersection.cpp new file mode 100644 index 0000000000..8317c6b095 --- /dev/null +++ b/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 +#include +#include + +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; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/mesh/MeshIntersection.hpp b/src/aliceVision/mesh/MeshIntersection.hpp new file mode 100644 index 0000000000..2320d8f424 --- /dev/null +++ b/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 + +#include +#include + +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; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index d9889cfd7c..823826aabd 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/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 diff --git a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp index a49608b57a..cb8705599d 100644 --- a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp +++ b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -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& 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 - // ObsToLandmark - using ObsKey = std::tuple; - using ObsToLandmark = std::map; - - 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() { diff --git a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.hpp b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.hpp index 835ed45d1b..9d9f8738f8 100644 --- a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.hpp +++ b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.hpp @@ -147,11 +147,6 @@ class ReconstructionEngine_sequentialSfM : public ReconstructionEngine */ void createInitialReconstruction(const std::vector& 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 diff --git a/src/aliceVision/sfm/utils/alignment.cpp b/src/aliceVision/sfm/utils/alignment.cpp index 2901bb999f..d61e739189 100644 --- a/src/aliceVision/sfm/utils/alignment.cpp +++ b/src/aliceVision/sfm/utils/alignment.cpp @@ -610,6 +610,121 @@ bool computeSimilarityFromCommonLandmarks(const sfmData::SfMData& sfmDataA, return true; } +bool computeSimilarityFromPairs(const std::vector & ptsA, + const std::vector & 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 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 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 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 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 */ diff --git a/src/aliceVision/sfm/utils/alignment.hpp b/src/aliceVision/sfm/utils/alignment.hpp index ae41edfdb4..86a8b947fb 100644 --- a/src/aliceVision/sfm/utils/alignment.hpp +++ b/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 & ptsA, + const std::vector & ptsB, + std::mt19937& randomNumberGenerator, + double* out_S, + Mat3* out_R, + Vec3* out_t); + /** * @brief Apply a transformation the given SfMData * diff --git a/src/aliceVision/sfm/utils/preprocess.cpp b/src/aliceVision/sfm/utils/preprocess.cpp new file mode 100644 index 0000000000..704302dd4b --- /dev/null +++ b/src/aliceVision/sfm/utils/preprocess.cpp @@ -0,0 +1,70 @@ +// 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 "preprocess.hpp" + +namespace aliceVision { +namespace sfm { + +void remapLandmarkIdsToTrackIds(sfmData::SfMData& sfmData, const track::TracksMap & tracks) +{ + using namespace track; + + // get unmap landmarks + sfmData::Landmarks landmarks; + + // clear sfmData structure and store them locally + std::swap(landmarks, sfmData.getLandmarks()); + + // builds landmarks temporary comparison structure + // ObsKey + // ObsToLandmark + using ObsKey = std::tuple; + using ObsToLandmark = std::map; + + 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 : 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: " << tracks.size() << std::endl + << "\t- # input landmarks: " << landmarks.size() << std::endl + << "\t- # output landmarks: " << sfmData.getLandmarks().size()); +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/utils/preprocess.hpp b/src/aliceVision/sfm/utils/preprocess.hpp new file mode 100644 index 0000000000..1095cc8707 --- /dev/null +++ b/src/aliceVision/sfm/utils/preprocess.hpp @@ -0,0 +1,21 @@ +// 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 +#include + +namespace aliceVision { +namespace sfm { + +/** + * @brief Make sure the landmarks ids are consistent with the track ids + * @param sfmData the sfmData to update + * @param tracks the input information about tracks to compare with landmarks +*/ +void remapLandmarkIdsToTrackIds(sfmData::SfMData& sfmData, const track::TracksMap & tracks); + +} +} \ No newline at end of file diff --git a/src/software/utils/CMakeLists.txt b/src/software/utils/CMakeLists.txt index 3c18845d56..447347c722 100644 --- a/src/software/utils/CMakeLists.txt +++ b/src/software/utils/CMakeLists.txt @@ -186,10 +186,12 @@ if(ALICEVISION_BUILD_SFM) LINKS aliceVision_system aliceVision_cmdline aliceVision_feature + aliceVision_mesh aliceVision_sfm aliceVision_sfmData aliceVision_sfmDataIO Boost::program_options + Boost::json ) # SfM to rig diff --git a/src/software/utils/main_sfmTransform.cpp b/src/software/utils/main_sfmTransform.cpp index 760912f607..b4f1708b08 100644 --- a/src/software/utils/main_sfmTransform.cpp +++ b/src/software/utils/main_sfmTransform.cpp @@ -7,17 +7,28 @@ #include #include #include +#include #include #include #include +#include +#include +#include #include +#include #include +#include #include #include #include #include +#include + + + + // These constants define the current software version. // They must be updated when the command line is changed. @@ -45,6 +56,7 @@ enum class EAlignmentMethod : unsigned char FROM_CENTER_CAMERA, FROM_MARKERS, FROM_GPS, + FROM_LINEUP, ALIGN_GROUND }; @@ -77,6 +89,8 @@ std::string EAlignmentMethod_enumToString(EAlignmentMethod alignmentMethod) return "from_markers"; case EAlignmentMethod::FROM_GPS: return "from_gps"; + case EAlignmentMethod::FROM_LINEUP: + return "from_lineup"; case EAlignmentMethod::ALIGN_GROUND: return "align_ground"; } @@ -113,6 +127,8 @@ EAlignmentMethod EAlignmentMethod_stringToEnum(const std::string& alignmentMetho return EAlignmentMethod::FROM_MARKERS; if (method == "from_gps") return EAlignmentMethod::FROM_GPS; + if (method == "from_lineup") + return EAlignmentMethod::FROM_LINEUP; if (method == "align_ground") return EAlignmentMethod::ALIGN_GROUND; throw std::out_of_range("Invalid SfM alignment method : " + alignmentMethod); @@ -251,6 +267,167 @@ IndexT getReferenceViewId(const sfmData::SfMData& sfmData, const std::string& tr return refViewId; } +bool getPoseFromJson(const std::string & lineUpFilename, Eigen::Matrix4d & T, IndexT & frameId) +{ + std::ifstream inputfile(lineUpFilename); + if (!inputfile.is_open()) + { + return false; + } + + std::stringstream buffer; + buffer << inputfile.rdbuf(); + boost::json::value jv = boost::json::parse(buffer.str()); + + const boost::json::object& obj = jv.as_object(); + + //Reading information from lineup + const double rx = degreeToRadian(boost::json::value_to(obj.at("rx"))); + const double ry = degreeToRadian(boost::json::value_to(obj.at("ry"))); + const double rz = degreeToRadian(boost::json::value_to(obj.at("rz"))); + const double tx = boost::json::value_to(obj.at("tx")); + const double ty = boost::json::value_to(obj.at("ty")); + const double tz = boost::json::value_to(obj.at("tz")); + + frameId = boost::json::value_to(obj.at("frame_no")); + + Eigen::AngleAxisd Rx(rx, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd Ry(ry, Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd Rz(rz, Eigen::Vector3d::UnitZ()); + + Eigen::Matrix3d R = Ry.toRotationMatrix() * Rx.toRotationMatrix() * Rz.toRotationMatrix(); + + Eigen::Vector3d t; + t.x() = tx; + t.y() = ty; + t.z() = tz; + + T = Eigen::Matrix4d::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t; + + return true; +} + + +bool parseLineUp(const std::string & lineUpFilename, const std::string & tracksFilename, const std::string & objectFilename, sfmData::SfMData& sfmData, double & S, Eigen::Matrix3d & R, Eigen::Vector3d & t) +{ + // Load new pose from file and the lined up frame id + IndexT frameId = UndefinedIndexT; + Eigen::Matrix4d inputTransform; + if (!getPoseFromJson(lineUpFilename, inputTransform, frameId)) + { + return false; + } + + //Find in the sfmData the view with the given frame id + IndexT viewId = UndefinedIndexT; + IndexT poseId = UndefinedIndexT; + IndexT intrinsicId = UndefinedIndexT; + for (const auto & pv : sfmData.getViews()) + { + if (pv.second->getFrameId() == frameId) + { + viewId = pv.first; + intrinsicId = pv.second->getIntrinsicId(); + poseId = pv.second->getPoseId(); + } + } + + // Make sure some view has been found + if (viewId == UndefinedIndexT) + { + ALICEVISION_LOG_INFO("Frame Number not found amongst views"); + return false; + } + + // Make sure the associated view has both pose and intrinsics calibrated + if (!sfmData.isPoseAndIntrinsicDefined(viewId)) + { + ALICEVISION_LOG_INFO("View has not been reconstructed, can't lineup"); + return false; + } + + // Load tracks + ALICEVISION_LOG_INFO("Load tracks"); + track::TracksHandler tracksHandler; + if (!tracksHandler.load(tracksFilename, sfmData.getValidViews())) + { + ALICEVISION_LOG_ERROR("The input tracks file '" + tracksFilename + "' cannot be read."); + return false; + } + + //Get all tracks for lineup view + const auto & tpv = tracksHandler.getTracksPerView(); + const auto & tracks = tracksHandler.getAllTracks(); + const auto & trackRefs = tpv.at(viewId); + + //Make sure landmarks are correctly identified + sfm::remapLandmarkIdsToTrackIds(sfmData, tracks); + + //Retrieve camera intrinsics + const auto & intrinsic = sfmData.getIntrinsics().at(intrinsicId); + + //Get transform in av coordinates + Eigen::Matrix4d aliceTinput = Eigen::Matrix4d::Identity(); + aliceTinput(1, 1) = -1; + aliceTinput(1, 2) = 0; + aliceTinput(2, 1) = 0; + aliceTinput(2, 2) = -1; + + const Eigen::Matrix4d newworld_T_frame = aliceTinput * inputTransform * aliceTinput.inverse(); + const Eigen::Matrix4d frame_T_newworld = newworld_T_frame.inverse(); + + auto & landmarks = sfmData.getLandmarks(); + + mesh::MeshIntersection meshIntersection; + if (!meshIntersection.initialize(objectFilename)) + { + return false; + } + + meshIntersection.setPose(frame_T_newworld); + + // Keep only tracks with reconstructed landmark observed in lineup view + std::vector landmarkCoordinates; + std::vector meshCoordinates; + for (auto trackId: trackRefs) + { + if (landmarks.find(trackId) == landmarks.end()) + { + continue; + } + + auto & l = landmarks.at(trackId); + if (l.getObservations().find(viewId) == l.getObservations().end()) + { + continue; + } + + const auto & track = tracks.at(trackId); + const auto & trackitem = track.featPerView.at(viewId); + const Vec2 & imageCoords = trackitem.coords; + + Vec3 pt3d; + if (!meshIntersection.peek(pt3d, *intrinsic, imageCoords)) + { + continue; + } + + landmarkCoordinates.push_back(landmarks.at(trackId).X); + meshCoordinates.push_back(pt3d); + } + + + std::mt19937 randomNumberGenerator; + if (!sfm::computeSimilarityFromPairs(landmarkCoordinates, meshCoordinates, randomNumberGenerator, &S, &R, &t)) + { + return false; + } + + return true; +} + int aliceVision_main(int argc, char** argv) { // command-line parameters @@ -269,6 +446,9 @@ int aliceVision_main(int argc, char** argv) std::string outputViewsAndPosesFilepath; std::string manualTransform; + std::string lineUpFilename = ""; + std::string tracksFilename = ""; + std::string objectFilename = ""; // clang-format off po::options_description requiredParams("Required parameters"); @@ -312,6 +492,12 @@ int aliceVision_main(int argc, char** argv) "Apply translation transformation.") ("markers", po::value>(&markers)->multitoken(), "Markers ID and target coordinates 'ID:x,y,z'.") + ("lineUp", po::value(&lineUpFilename)->default_value(lineUpFilename), + "LineUp file used as information.") + ("tracksFile", po::value(&tracksFilename)->default_value(tracksFilename), + "tracks file used as information for lineup.") + ("objectFile", po::value(&objectFilename)->default_value(objectFilename), + "object file used as information for lineup.") ("outputViewsAndPoses", po::value(&outputViewsAndPosesFilepath), "Path of the output SfMData file."); // clang-format on @@ -485,6 +671,15 @@ int aliceVision_main(int argc, char** argv) } break; } + case EAlignmentMethod::FROM_LINEUP: + { + if (!parseLineUp(lineUpFilename, tracksFilename, objectFilename, sfmData, S, R, t)) + { + ALICEVISION_LOG_ERROR("Failed to use given lineup"); + return EXIT_FAILURE; + } + break; + } case EAlignmentMethod::ALIGN_GROUND: { sfm::computeNewCoordinateSystemGroundAuto(sfmData, t);