diff --git a/src/aliceVision/keyframe/KeyframeSelector.cpp b/src/aliceVision/keyframe/KeyframeSelector.cpp index aa7bb2b298..9fbab98efc 100644 --- a/src/aliceVision/keyframe/KeyframeSelector.cpp +++ b/src/aliceVision/keyframe/KeyframeSelector.cpp @@ -1252,6 +1252,8 @@ bool KeyframeSelector::writeSfMData(const std::string& mediaPath, bool KeyframeSelector::writeSfMDataFromSfMData(const std::string& mediaPath) { + auto& keyframesPoses = _outputSfmKeyframes.getPoses(); + auto& keyframesViews = _outputSfmKeyframes.getViews(); auto& framesViews = _outputSfmFrames.getViews(); @@ -1292,6 +1294,18 @@ bool KeyframeSelector::writeSfMDataFromSfMData(const std::string& mediaPath) { viewId = views[i]->getViewId(); intrinsicId = views[i]->getIntrinsicId(); + IndexT poseId = views[i]->getPoseId(); + + bool hasPose = inputSfm.isPoseAndIntrinsicDefined(viewId); + if (hasPose) + { + ALICEVISION_LOG_INFO("A fully calibrated view (Id:" << viewId << ") is moved to keyframes"); + _selectedFrames[i] = '1'; + + //Make sure to keep the pose + keyframesPoses.emplace(poseId, inputSfm.getPoses().at(poseId)); + } + if (_selectedFrames[i] == '1') { keyframesViews.emplace(viewId, views[i]); diff --git a/src/aliceVision/mesh/MeshIntersection.cpp b/src/aliceVision/mesh/MeshIntersection.cpp index b173cec24b..2c7d8e6189 100644 --- a/src/aliceVision/mesh/MeshIntersection.cpp +++ b/src/aliceVision/mesh/MeshIntersection.cpp @@ -28,7 +28,7 @@ bool MeshIntersection::initialize(const std::string & pathToModel) return true; } -bool MeshIntersection::peekPoint(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords) +bool MeshIntersection::pickPoint(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords) { const Vec3 posCamera = _pose.center(); const Vec3 wdir = intrinsic.backprojectTransform(imageCoords, true, _pose, 1.0); @@ -58,7 +58,7 @@ bool MeshIntersection::peekPoint(Vec3 & output, const camera::IntrinsicBase & in return true; } -bool MeshIntersection::peekNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords) +bool MeshIntersection::pickNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords) { const Vec3 posCamera = _pose.center(); const Vec3 wdir = intrinsic.backprojectTransform(imageCoords, true, _pose, 1.0); @@ -88,5 +88,41 @@ bool MeshIntersection::peekNormal(Vec3 & output, const camera::IntrinsicBase & i return true; } +bool MeshIntersection::pickPointAndNormal(Vec3 & point, Vec3 & normal, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords) +{ + const Vec3 posCamera = _pose.center(); + const Vec3 wdir = intrinsic.backprojectTransform(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; + + point.x() = p.x; + point.y() = -p.y; + point.z() = -p.z; + + const GEO::vec3 n = GEO::normalize(intersection.N); + + normal.x() = n.x; + normal.y() = -n.y; + normal.z() = -n.z; + + return true; +} + } } \ No newline at end of file diff --git a/src/aliceVision/mesh/MeshIntersection.hpp b/src/aliceVision/mesh/MeshIntersection.hpp index a0936cc526..c15182d3d6 100644 --- a/src/aliceVision/mesh/MeshIntersection.hpp +++ b/src/aliceVision/mesh/MeshIntersection.hpp @@ -22,7 +22,7 @@ class MeshIntersection bool initialize(const std::string & pathToModel); /** - * @brief Update pose to use for peeking + * @brief Update pose to use for picking * @param pose transformation to use (in aliceVision standard form) */ void setPose(const geometry::Pose3 & pose) @@ -31,22 +31,32 @@ class MeshIntersection } /** - * @brief peek a point on the mesh given a input camera observation + * @brief pick 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. + * @return true if the ray intersects the mesh. */ - bool peekPoint(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords); + bool pickPoint(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords); /** - * @brief peek a point and get its normal on the mesh given a input camera observation + * @brief pick a point and get its normal on the mesh given a input camera observation * @param output the output measured normal * @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. + * @return true if the ray intersects the mesh. */ - bool peekNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords); + bool pickNormal(Vec3 & output, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords); + + /** + * @brief pick a point and get its normal on the mesh given a input camera observation + * @param point the output measured point + * @param normal the output measured normal + * @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 intersects the mesh. + */ + bool pickPointAndNormal(Vec3 & point, Vec3 & normal, const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords); private: GEO::Mesh _mesh; diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index 529b729924..b0b1d659de 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -24,6 +24,9 @@ set(sfm_files_headers pipeline/RelativePoseInfo.hpp pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.hpp pipeline/panorama/ReconstructionEngine_panorama.hpp + pipeline/bootstrapping/EstimateAngle.hpp + pipeline/bootstrapping/PairsScoring.hpp + pipeline/bootstrapping/Bootstrap.hpp pipeline/expanding/SfmTriangulation.hpp pipeline/expanding/SfmResection.hpp pipeline/expanding/SfmBundle.hpp @@ -65,6 +68,9 @@ set(sfm_files_sources pipeline/RelativePoseInfo.cpp pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp pipeline/panorama/ReconstructionEngine_panorama.cpp + pipeline/bootstrapping/EstimateAngle.cpp + pipeline/bootstrapping/PairsScoring.cpp + pipeline/bootstrapping/Bootstrap.cpp pipeline/expanding/SfmTriangulation.cpp pipeline/expanding/SfmResection.cpp pipeline/expanding/SfmBundle.cpp diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index 447a46dd9b..bc8f973ae4 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -86,6 +87,17 @@ ceres::CostFunction* createConstraintsCostFunctionFromIntrinsics(std::shared_ptr return costFunction; } +ceres::CostFunction* createCostFunctionFromContraintPoint(const sfmData::Landmark & landmark, const Vec3 & normal) +{ + const double weight = 100.0; + auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ConstraintPointErrorFunctor(weight, normal, landmark.X)); + + costFunction->AddParameterBlock(3); + costFunction->SetNumResiduals(1); + + return costFunction; +} + void BundleAdjustmentCeres::CeresOptions::setDenseBA() { // default configuration use a DENSE representation @@ -671,6 +683,33 @@ void BundleAdjustmentCeres::addConstraints2DToProblem(const sfmData::SfMData& sf } } +void BundleAdjustmentCeres::addConstraintsPointToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) +{ + // set a LossFunction to be less penalized by false measurements. + // note: set it to NULL if you don't want use a lossFunction. + ceres::LossFunction* lossFunction = _ceresOptions.lossFunction.get(); + + for (const auto& [landmarkId, constraint] : sfmData.getConstraintsPoint()) + { + if (sfmData.getLandmarks().find(landmarkId) == sfmData.getLandmarks().end()) + { + continue; + } + + if (_landmarksBlocks.find(landmarkId) == _landmarksBlocks.end()) + { + continue; + } + + const sfmData::Landmark & l = sfmData.getLandmarks().at(landmarkId); + double * ldata = _landmarksBlocks.at(landmarkId).data(); + + ceres::CostFunction* costFunction = createCostFunctionFromContraintPoint(l, constraint.normal); + + problem.AddResidualBlock(costFunction, lossFunction, ldata); + } +} + void BundleAdjustmentCeres::addRotationPriorsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) { // set a LossFunction to be less penalized by false measurements. @@ -718,6 +757,9 @@ void BundleAdjustmentCeres::createProblem(const sfmData::SfMData& sfmData, ERefi // add 2D constraints to the Ceres problem addConstraints2DToProblem(sfmData, refineOptions, problem); + // add 2D constraints to the Ceres problem + addConstraintsPointToProblem(sfmData, refineOptions, problem); + // add rotation priors to the Ceres problem addRotationPriorsToProblem(sfmData, refineOptions, problem); } diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp index 1a87ad943b..b575441972 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp @@ -189,6 +189,14 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback */ void addConstraints2DToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); + /** + * @brief Create a residual block for each point constraints + * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics + * @param[in] refineOptions The chosen refine flag + * @param[out] problem The Ceres bundle adjustement problem + */ + void addConstraintsPointToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); + /** * @brief Create a residual block for each rotation priors * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics diff --git a/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp b/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp new file mode 100644 index 0000000000..f4ef19a2f7 --- /dev/null +++ b/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp @@ -0,0 +1,39 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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/. + +#pragma once + +#include + +namespace aliceVision { +namespace sfm { + +struct ConstraintPointErrorFunctor +{ + explicit ConstraintPointErrorFunctor(const double weight, const Vec3 & normal, const Vec3 & point) + : _weight(weight), _normal(normal) + { + _constraintDistance = _normal.dot(point); + } + + template + bool operator()(T const* const* parameters, T* residuals) const + { + const T* parameter_point = parameters[0]; + + T distance = parameter_point[0] * _normal[0] + parameter_point[1] * _normal[1] + parameter_point[2] * _normal[2]; + residuals[0] = _weight * (distance - _constraintDistance); + + return true; + } + + double _weight; + Vec3 _normal; + double _constraintDistance; +}; + +} // namespace sfm +} // namespace aliceVision diff --git a/src/aliceVision/sfm/pipeline/bootstrapping/Bootstrap.cpp b/src/aliceVision/sfm/pipeline/bootstrapping/Bootstrap.cpp new file mode 100644 index 0000000000..2e8287bbf0 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/bootstrapping/Bootstrap.cpp @@ -0,0 +1,170 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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 +#include +#include +#include + +namespace aliceVision { +namespace sfm { + +bool bootstrapBase(sfmData::SfMData & sfmData, + const IndexT referenceViewId, + const IndexT otherViewId, + const geometry::Pose3 & otherTreference, + const track::TracksMap& tracksMap, + const track::TracksPerView & tracksPerView) +{ + const sfmData::View& refView = sfmData.getView(referenceViewId); + const sfmData::View& nextView = sfmData.getView(otherViewId); + + std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); + std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); + + sfmData::CameraPose cposeNext(otherTreference, false); + sfmData.getPoses()[refView.getPoseId()] = sfmData::CameraPose(); + sfmData.getPoses()[nextView.getPoseId()] = cposeNext; + + const Mat4 T1 = Eigen::Matrix4d::Identity(); + Mat4 T2 = otherTreference.getHomogeneous(); + + aliceVision::track::TracksMap mapTracksCommon; + track::getCommonTracksInImagesFast({referenceViewId, otherViewId}, tracksMap, tracksPerView, mapTracksCommon); + + size_t count = 0; + std::vector angles; + for(const auto& [trackId, track] : mapTracksCommon) + { + const track::TrackItem & refItem = track.featPerView.at(referenceViewId); + const track::TrackItem & nextItem = track.featPerView.at(otherViewId); + + const Vec2 refpt = refItem.coords; + const Vec2 nextpt = nextItem.coords; + + const Vec3 pt3d1 = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refpt))); + const Vec3 pt3d2 = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextpt))); + + + Vec3 X; + multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X); + + Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); + Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); + if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) + { + continue; + } + + sfmData::Landmark landmark; + landmark.descType = track.descType; + landmark.X = X; + + sfmData::Observation refObs; + refObs.setFeatureId(refItem.featureId); + refObs.setScale(refItem.scale); + refObs.setCoordinates(refItem.coords); + + sfmData::Observation nextObs; + nextObs.setFeatureId(nextItem.featureId); + nextObs.setScale(nextItem.scale); + nextObs.setCoordinates(nextItem.coords); + + landmark.getObservations()[referenceViewId] = refObs; + landmark.getObservations()[otherViewId] = nextObs; + + sfmData.getLandmarks()[trackId] = landmark; + } + + return true; +} + +bool bootstrapMesh(sfmData::SfMData & sfmData, + const sfmData::Landmarks & landmarks, + const IndexT referenceViewId, + const IndexT nextViewId, + const track::TracksMap& tracksMap, + const track::TracksPerView & tracksPerView) +{ + //Select which view to resect + IndexT viewId = referenceViewId; + if (sfmData.isPoseAndIntrinsicDefined(viewId)) + { + viewId = nextViewId; + } + + //Assign new landmarks to sfmData to allow resection to work + sfmData.getLandmarks() = landmarks; + + std::mt19937 randomNumberGenerator; + Eigen::Matrix4d pose; + double threshold; + size_t countInliers; + + //Compute resection for selected view + SfmResection resection(50000, std::numeric_limits::infinity()); + if (!resection.processView(sfmData, tracksMap, tracksPerView, randomNumberGenerator, viewId, pose, threshold, countInliers)) + { + return false; + } + + ALICEVISION_LOG_INFO("Resection succeeded with a threshold of " << threshold); + + + //Prepare objects for second view + const sfmData::View& view = sfmData.getView(viewId); + std::shared_ptr intrinsics = sfmData.getIntrinsicSharedPtr(view.getIntrinsicId()); + + geometry::Pose3 pose3(pose); + sfmData::CameraPose cpose(pose3, false); + sfmData.getPoses()[viewId] = cpose; + + // Cleanup output + sfmData::Landmarks & outLandmarks = sfmData.getLandmarks(); + outLandmarks.clear(); + + for (const auto & [landmarkId, landmark] : landmarks) + { + //Retrieve track object + const auto & track = tracksMap.at(landmarkId); + + //Maybe this track is not observed in the next view + if (track.featPerView.find(viewId) == track.featPerView.end()) + { + continue; + } + + //Compute error + const track::TrackItem & item = track.featPerView.at(viewId); + const Vec2 pt = item.coords; + const Vec2 estpt = intrinsics->transformProject(pose3, landmark.X.homogeneous(), true); + double err = (pt - estpt).norm(); + + //If error is ok, then we add it to the sfmData + if (err <= threshold) + { + sfmData::Observation obs; + obs.setFeatureId(item.featureId); + obs.setScale(item.scale); + obs.setCoordinates(item.coords); + + + //Add landmark to sfmData + outLandmarks[landmarkId] = landmark; + + //Add observation to landmark + sfmData::Observations & observations = outLandmarks[landmarkId].getObservations(); + observations[nextViewId] = obs; + } + } + + return true; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/bootstrapping/Bootstrap.hpp b/src/aliceVision/sfm/pipeline/bootstrapping/Bootstrap.hpp new file mode 100644 index 0000000000..ceb8c7067b --- /dev/null +++ b/src/aliceVision/sfm/pipeline/bootstrapping/Bootstrap.hpp @@ -0,0 +1,50 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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 sfm { + +/** + * @brief Create a minimal SfmData with poses and landmarks for two views + * @param sfmData the input sfmData which contains camera information + * @param referenceViewId the reference view id + * @param otherViewId the other view id + * @param otherTreference the relative pose + * @param tracksMap the input map of tracks + * @param tracksPerView tracks grouped by views + * @return true +*/ +bool bootstrapBase(sfmData::SfMData & sfmData, + const IndexT referenceViewId, + const IndexT otherViewId, + const geometry::Pose3 & otherTreference, + const track::TracksMap& tracksMap, + const track::TracksPerView & tracksPerView); + +/** + * @brief Create a minimal SfmData with poses and landmarks for two views (given a mesh) + * @param sfmData the input sfmData which contains camera information + * @param landmarks the input Landmarks file which contains mesh geometry + * @param referenceViewId the reference view id + * @param nextViewId the reference view id + * @param tracksMap the input map of tracks + * @param tracksPerView tracks grouped by views + * @return true +*/ +bool bootstrapMesh(sfmData::SfMData & sfmData, + const sfmData::Landmarks & landmarks, + const IndexT referenceViewId, + const IndexT nextViewId, + const track::TracksMap& tracksMap, + const track::TracksPerView & tracksPerView); + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/bootstrapping/EstimateAngle.cpp b/src/aliceVision/sfm/pipeline/bootstrapping/EstimateAngle.cpp new file mode 100644 index 0000000000..de9c6eeb94 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/bootstrapping/EstimateAngle.cpp @@ -0,0 +1,89 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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 sfm { + +bool estimatePairAngle(const sfmData::SfMData & sfmData, + const IndexT referenceViewId, + const IndexT otherViewId, + const geometry::Pose3 & otherTreference, + const track::TracksMap& tracksMap, + const track::TracksPerView & tracksPerView, + double & resultAngle, + std::vector & usedTracks) +{ + usedTracks.clear(); + + const sfmData::View& refView = sfmData.getView(referenceViewId); + const sfmData::View& nextView = sfmData.getView(otherViewId); + + std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); + std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); + + aliceVision::track::TracksMap mapTracksCommon; + track::getCommonTracksInImagesFast({referenceViewId, otherViewId}, tracksMap, tracksPerView, mapTracksCommon); + + const Mat4 T1 = Eigen::Matrix4d::Identity(); + const Mat4 T2 = otherTreference.getHomogeneous(); + + const Eigen::Vector3d c = otherTreference.center(); + + size_t count = 0; + std::vector angles; + for(const auto& commonItem : mapTracksCommon) + { + const track::Track& track = commonItem.second; + + const IndexT refFeatureId = track.featPerView.at(referenceViewId).featureId; + const IndexT nextfeatureId = track.featPerView.at(otherViewId).featureId; + + const Vec2 refpt = track.featPerView.at(referenceViewId).coords; + const Vec2 nextpt = track.featPerView.at(otherViewId).coords; + + const Vec3 pt3d1 = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refpt))); + const Vec3 pt3d2 = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextpt))); + + + Vec3 X; + multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X); + + //Make sure + Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); + Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); + if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) + { + continue; + } + + const Vec3 ray1 = - X; + const Vec3 ray2 = c - X; + const double cangle = clamp(ray1.normalized().dot(ray2.normalized()), -1.0, 1.0); + const double angle = std::acos(cangle); + angles.push_back(angle); + + usedTracks.push_back(commonItem.first); + } + + if (angles.size() == 0) + { + resultAngle = 0.0; + return false; + } + + const unsigned medianIndex = angles.size() / 2; + std::nth_element(angles.begin(), angles.begin() + medianIndex, angles.end()); + resultAngle = angles[medianIndex]; + return true; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/bootstrapping/EstimateAngle.hpp b/src/aliceVision/sfm/pipeline/bootstrapping/EstimateAngle.hpp new file mode 100644 index 0000000000..19b12ed3a2 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/bootstrapping/EstimateAngle.hpp @@ -0,0 +1,37 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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 sfm { + +/** + * @brief estimate a median angle (parallax) between a reference view and another view + * @param sfmData the input sfmData which contains camera information + * @param referenceViewId the reference view id + * @param otherViewId the other view id + * @param otherTreference the relative pose + * @param tracksMap the input map of tracks + * @param tracksPerView tracks grouped by views + * @param resultAngle the output median angle + * @param usedTracks the list of tracks which were succesfully reconstructed + * @return true +*/ +bool estimatePairAngle(const sfmData::SfMData & sfmData, + const IndexT referenceViewId, + const IndexT otherViewId, + const geometry::Pose3 & otherTreference, + const track::TracksMap& tracksMap, + const track::TracksPerView & tracksPerView, + double & resultAngle, + std::vector & usedTracks); + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/bootstrapping/PairsScoring.cpp b/src/aliceVision/sfm/pipeline/bootstrapping/PairsScoring.cpp new file mode 100644 index 0000000000..6dc89a14b8 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/bootstrapping/PairsScoring.cpp @@ -0,0 +1,71 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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 +#include + +namespace aliceVision { +namespace sfm { + +IndexT findBestPair(const sfmData::SfMData & sfmData, + const std::vector & pairs, + const track::TracksMap& tracksMap, + const track::TracksPerView & tracksPerView, + double minAngle, + double maxAngle) +{ + IndexT bestPair = UndefinedIndexT; + double maxScore = std::numeric_limits::lowest(); + + for (IndexT pairId = 0; pairId < pairs.size(); pairId++) + { + const sfm::ReconstructedPair & pair = pairs[pairId]; + + double angle = 0.0; + std::vector usedTracks; + if (!sfm::estimatePairAngle(sfmData, pair.reference, pair.next, pair.pose, tracksMap, tracksPerView, angle, usedTracks)) + { + continue; + } + + if (radianToDegree(angle) > maxAngle) + { + continue; + } + + + + const sfmData::View & vref = sfmData.getView(pair.reference); + const sfmData::View & vnext = sfmData.getView(pair.next); + + int maxref = std::max(vref.getImage().getWidth(), vref.getImage().getHeight()); + int maxnext = std::max(vnext.getImage().getWidth(), vnext.getImage().getHeight()); + + + double refScore = sfm::ExpansionPolicyLegacy::computeScore(tracksMap, usedTracks, pair.reference, maxref, 5); + double nextScore = sfm::ExpansionPolicyLegacy::computeScore(tracksMap, usedTracks, pair.next, maxnext, 5); + + double score = std::min(refScore, nextScore) * std::max(1.0, radianToDegree(angle)); + //If the angle is too small, then dramatically reduce its chances + if (radianToDegree(angle) < minAngle) + { + score = -1.0 / score; + } + + if (score > maxScore) + { + maxScore = score; + bestPair = pairId; + } + } + + return bestPair; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/bootstrapping/PairsScoring.hpp b/src/aliceVision/sfm/pipeline/bootstrapping/PairsScoring.hpp new file mode 100644 index 0000000000..6da3e732f0 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/bootstrapping/PairsScoring.hpp @@ -0,0 +1,35 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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/. +#pragma once + +#include +#include +#include + +#include + +namespace aliceVision { +namespace sfm { + +/** +* @brief Get best pair with highest score +* @param sfmData the input sfmData which contains camera information +* @param pairs the input list of reconstructed pairs +* @param tracksMap the input map of tracks +* @param tracksPerView tracks grouped by views +* @param minAngle minimal angle allowed +* @param maxAngle maximal angle allowed +* @return index in "pairs" of the best pair or UndefinedIndexT if no pair found +*/ +IndexT findBestPair(const sfmData::SfMData & sfmData, + const std::vector & pairs, + const track::TracksMap& tracksMap, + const track::TracksPerView & tracksPerView, + double minAngle, + double maxAngle); + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp index bc821e5b64..8f892a40b5 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp @@ -15,6 +15,7 @@ namespace sfm { bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewsChunk) { + _ignoredViews.clear(); ALICEVISION_LOG_INFO("ExpansionChunk::process start"); ALICEVISION_LOG_INFO("Chunk size : " << viewsChunk.size()); @@ -31,6 +32,17 @@ bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHand return false; } + + struct IntermediateResectionInfo + { + IndexT viewId; + Eigen::Matrix4d pose; + size_t inliersCount; + double threshold; + }; + + std::vector intermediateInfos; + ALICEVISION_LOG_INFO("Resection start"); #pragma omp parallel for for (int i = 0; i < viewsChunk.size(); i++) @@ -41,25 +53,52 @@ bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHand if (!sfmData.isPoseAndIntrinsicDefined(viewId)) { + IntermediateResectionInfo iri; + iri.viewId = viewId; + SfmResection resection(_resectionIterations, _resectionMaxError); - Eigen::Matrix4d pose; - double threshold = 0.0; std::mt19937 randomNumberGenerator; if (!resection.processView(sfmData, tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), randomNumberGenerator, viewId, - pose, threshold)) + iri.pose, iri.threshold, iri.inliersCount)) { continue; } #pragma omp critical { - - addPose(sfmData, viewId, pose); + intermediateInfos.push_back(iri); + } + } + } + + //Check that at least one view has rich info + const int poorInliersCount = 100; + int richViews = 0; + for (const auto & item : intermediateInfos) + { + if (item.inliersCount > poorInliersCount) + { + richViews++; + } + } + + + //Add pose only if it match conditions + for (const auto & item : intermediateInfos) + { + if (richViews > 0) + { + if (item.inliersCount < poorInliersCount) + { + _ignoredViews.insert(item.viewId); + continue; } } + + addPose(sfmData, item.viewId, item.pose); } // Get a list of valid views @@ -86,6 +125,11 @@ bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHand return false; } + if (_pointFetcherHandler) + { + setConstraints(sfmData, tracksHandler, validViewIds); + } + if (_historyHandler) { _historyHandler->saveState(sfmData); @@ -158,6 +202,125 @@ void ExpansionChunk::addPose(sfmData::SfMData & sfmData, IndexT viewId, const Ei sfmData.setPose(v, cpose); } +void ExpansionChunk::setConstraints(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) +{ + ALICEVISION_LOG_INFO("ExpansionChunk::setConstraints start"); + const track::TracksMap & tracks = tracksHandler.getAllTracks(); + const track::TracksPerView & tracksPerView = tracksHandler.getTracksPerView(); + + const sfmData::Landmarks & landmarks = sfmData.getLandmarks(); + sfmData::ConstraintsPoint & constraints = sfmData.getConstraintsPoint(); + + std::map>> infoPerLandmark; + + // Fetch all points and normals and store them for further voting + for (const auto & viewId: sfmData.getValidViews()) + { + const sfmData::View & v = sfmData.getView(viewId); + const sfmData::CameraPose & cp = sfmData.getAbsolutePose(v.getPoseId()); + const camera::IntrinsicBase & intrinsics = *sfmData.getIntrinsics().at(v.getIntrinsicId()); + + _pointFetcherHandler->setPose(cp.getTransform()); + + const auto & trackIds = tracksPerView.at(viewId); + for (const auto trackId : trackIds) + { + const track::Track & track = tracks.at(trackId); + const track::TrackItem & trackItem = track.featPerView.at(viewId); + + if (landmarks.find(trackId) == landmarks.end()) + { + continue; + } + + Vec3 point, normal; + if (!_pointFetcherHandler->pickPointAndNormal(point, normal, intrinsics, trackItem.coords)) + { + continue; + } + + infoPerLandmark[trackId].push_back(std::make_pair(point, normal)); + } + } + + //Find the consensus + const double maxDist = 0.1; + const double maxDistLandmark = 1.0; + const double cosMaxAngle = cos(M_PI_4); + + for (const auto & [trackId, vecInfo] : infoPerLandmark) + { + if (vecInfo.empty()) + { + continue; + } + + int idBest = -1; + int countBest = -1; + + + //Consider each point + for (int idRef = 0; idRef < vecInfo.size(); idRef++) + { + const Vec3 & refpt = vecInfo[idRef].first; + const Vec3 & refnormal = vecInfo[idRef].second; + + //Compare it with all other points + int count = 0; + for (int idCur = 0; idCur < vecInfo.size(); idCur++) + { + if (idCur == idRef) + { + continue; + } + + const Vec3 & curpt = vecInfo[idRef].first; + const Vec3 & curnormal = vecInfo[idRef].second; + + double dist = (refpt - curpt).norm(); + if (dist > maxDist) + { + continue; + } + + if (curnormal.dot(refnormal) < cosMaxAngle) + { + continue; + } + + count++; + } + + if (count > countBest) + { + idBest = idRef; + countBest = count; + } + } + + const auto & landmark = landmarks.at(trackId); + const Vec3 point = vecInfo[idBest].first; + const Vec3 normal = vecInfo[idBest].second; + + double dist = (point - landmark.X).norm(); + if (dist > maxDistLandmark) + { + continue; + } + + if (idBest < 0) + { + ALICEVISION_THROW_ERROR("Impossible value"); + } + + sfmData::ConstraintPoint cp(trackId, normal, point); + constraints[trackId] = cp; + } + + ALICEVISION_LOG_INFO("ExpansionChunk::setConstraints added " << constraints.size() << " constraints"); + ALICEVISION_LOG_INFO("ExpansionChunk::setConstraints end"); +} + } // namespace sfm } // namespace aliceVision diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp index d54454aa01..3737d087bc 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.hpp @@ -11,6 +11,7 @@ #include #include #include +#include namespace aliceVision { namespace sfm { @@ -34,7 +35,7 @@ class ExpansionChunk const std::set & viewsChunk); /** - * brief setup the bundle handler + * @brief setup the bundle handler * @param bundleHandler a unique ptr. the Ownership will be taken */ void setBundleHandler(SfmBundle::uptr & bundleHandler) @@ -51,6 +52,15 @@ class ExpansionChunk _historyHandler = expansionHistory; } + /** + * brief setup the point fetcher handler + * @param pointFetcher a unique ptr. the Ownership will be taken + */ + void setPointFetcherHandler(PointFetcher::uptr & pointFetcherHandler) + { + _pointFetcherHandler = std::move(pointFetcherHandler); + } + void setResectionMaxIterations(size_t maxIterations) { _resectionIterations = maxIterations; @@ -88,6 +98,11 @@ class ExpansionChunk _minTriangulationAngleDegrees = angle; } + const std::set & getIgnoredViews() + { + return _ignoredViews; + } + private: /** @@ -106,9 +121,19 @@ class ExpansionChunk */ bool triangulate(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds); + /** + * @brief Add constraints on points + * @param sfmData the object to update + * @param tracks all tracks of the scene as a map {trackId, track} + * @param viewIds the set of views to process + */ + void setConstraints(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds); + private: SfmBundle::uptr _bundleHandler; ExpansionHistory::sptr _historyHandler; + PointFetcher::uptr _pointFetcherHandler; + std::set _ignoredViews; private: size_t _resectionIterations = 1024; diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.cpp index e2988a8e20..954defcea1 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionIteration.cpp @@ -47,6 +47,10 @@ bool ExpansionIteration::process(sfmData::SfMData & sfmData, track::TracksHandle continue; } + //Rollback any views which were ignored (not with errors) + _policy->rollback(_chunkHandler->getIgnoredViews()); + + //Save this epoch to history _historyHandler->endEpoch(sfmData, _policy->getNextViews()); } diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicy.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicy.hpp index 27f68bec08..fff36ebd7c 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicy.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicy.hpp @@ -33,6 +33,12 @@ class ExpansionPolicy * @return true if the policy succeeded */ virtual bool process(const sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler) = 0; + + /** + * @brief rollback some processed views inside the available views + * @param viewsSet the set of views that we want to be able to select again. + */ + virtual void rollback(const std::set & viewsSet) = 0; /** * @brief Retrieve the selected next views diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp index b30fd7a9c0..9a9279f9c8 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp @@ -200,5 +200,14 @@ double ExpansionPolicyLegacy::computeScore(const track::TracksMap & tracksMap, return sum; } +void ExpansionPolicyLegacy::rollback(const std::set & viewsSet) +{ + for (const auto & item : viewsSet) + { + ALICEVISION_LOG_INFO("Rollback view : " << item); + _availableViewsIds.insert(item); + } +} + } } \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp index 25361f2f8d..34afa986eb 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp @@ -73,6 +73,12 @@ class ExpansionPolicyLegacy : public ExpansionPolicy _maxViewsPerGroup = count; } + /** + * @brief rollback some processed views inside the available views + * @param viewsSet the set of views that we want to be able to select again. + */ + virtual void rollback(const std::set & viewsSet); + private: // vector of selected views for this iteration diff --git a/src/aliceVision/sfm/pipeline/expanding/PointFetcher.hpp b/src/aliceVision/sfm/pipeline/expanding/PointFetcher.hpp new file mode 100644 index 0000000000..089cc7f1ce --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/PointFetcher.hpp @@ -0,0 +1,44 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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/. + +#pragma once + +#include +#include + +namespace aliceVision +{ +namespace sfm +{ + +class PointFetcher +{ +public: + using uptr = std::unique_ptr; + +public: + /** + * Set the pose of the camera + * @param pose the pose of the camera wrt some global coordinates frame + */ + virtual void setPose(const geometry::Pose3 & pose) = 0; + + /** + * @brief virtual method to get coordinates and normals of a pixel of an image + * @param point result point in some global coordinates frame + * @param normal result normal in some global coordinates frame + * @param intrinsic the camera intrinsic object + * @param imageCoords the input image pixel coordinates in 2D. + * @return false on error + */ + virtual bool pickPointAndNormal(Vec3 & point, + Vec3 & normal, + const camera::IntrinsicBase & intrinsic, + const Vec2 & imageCoords) = 0; +}; + +} +} \ No newline at end of file diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp index 224e26f50b..d4ef853489 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp @@ -54,16 +54,20 @@ bool SfmBundle::cleanup(sfmData::SfMData & sfmData) // Remove landmarks without enough observed parallax const std::size_t nbOutliersAngleErr = removeOutliersWithAngleError(sfmData, _minAngleForLandmark); + // Remove constraints which are too far away from landmark + const std::size_t nbOutliersConstraints = removeConstraints(sfmData, _maxConstraintDistance); + // Remove poses without enough observations in an interative fashion const std::size_t nbOutliers = nbOutliersResidualErr + nbOutliersAngleErr; std::set removedViewsIdIteration; bool somethingErased = eraseUnstablePosesAndObservations(sfmData, _minPointsPerPose, _minTrackLength, &removedViewsIdIteration); - bool somethingChanged = /*somethingErased || */(nbOutliers > _bundleAdjustmentMaxOutlier); + bool somethingChanged = /*somethingErased || */(nbOutliers > _bundleAdjustmentMaxOutlier) || (nbOutliersConstraints > 0); ALICEVISION_LOG_INFO("SfmBundle::cleanup : "); ALICEVISION_LOG_INFO(" - nbOutliersResidualErr : " << nbOutliersResidualErr); ALICEVISION_LOG_INFO(" - nbOutliersAngleErr : " << nbOutliersAngleErr); + ALICEVISION_LOG_INFO(" - nbOutliersConstraints : " << nbOutliersConstraints); ALICEVISION_LOG_INFO(" - somethingErased : " << somethingErased); ALICEVISION_LOG_INFO(" - somethingChanged : " << somethingChanged); diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp index 445617ff8f..13f902a92a 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp @@ -94,6 +94,7 @@ class SfmBundle EFeatureConstraint _featureConstraint = EFeatureConstraint::SCALE; double _maxReprojectionError = 4.0; double _minAngleForLandmark = 2.0; + double _maxConstraintDistance = 1.0; size_t _minTrackLength = 2; size_t _minPointsPerPose = 30; size_t _bundleAdjustmentMaxOutlier = 50; diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp index 60d8b9bd61..d529fe33df 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp @@ -26,7 +26,8 @@ bool SfmResection::processView( std::mt19937 &randomNumberGenerator, const IndexT viewId, Eigen::Matrix4d & updatedPose, - double & updatedThreshold + double & updatedThreshold, + size_t & inliersCount ) { ALICEVISION_LOG_INFO("SfmResection::processView start " << viewId); @@ -90,12 +91,16 @@ bool SfmResection::processView( return false; } + inliersCount = inliers.size(); + ALICEVISION_LOG_INFO("Resection for view " << viewId << " had " << inliersCount << " inliers."); + //Refine the pose - if (!internalRefinement(structure, observations, inliers, pose, intrinsic)) + if (!internalRefinement(structure, observations, inliers, pose, intrinsic, errorMax)) { ALICEVISION_LOG_INFO("SfmResection::processView internalRefinemanet failed " << viewId); return false; } + updatedThreshold = errorMax; updatedPose = pose; @@ -140,7 +145,8 @@ bool SfmResection::internalRefinement( const std::vector & observations, const std::vector & inliers, Eigen::Matrix4d & pose, - std::shared_ptr & intrinsics) + std::shared_ptr & intrinsics, + double & errorMax) { // Setup a tiny SfM scene with the corresponding 2D-3D data sfmData::SfMData tinyScene; @@ -155,7 +161,7 @@ bool SfmResection::internalRefinement( // Intrinsics tinyScene.getIntrinsics().emplace(0, intrinsics); - const double unknownScale = 0.0; + const double unknownScale = 1.0; // structure data (2D-3D correspondences) for(std::size_t i = 0; i < inliers.size(); ++i) @@ -179,8 +185,21 @@ bool SfmResection::internalRefinement( pose = tinyScene.getPose(*view).getTransform().getHomogeneous(); + // Compute errorMax + errorMax = 0.0; + for(std::size_t i = 0; i < inliers.size(); ++i) + { + const std::size_t idx = inliers[i]; + + Vec2 est = intrinsics->transformProject(pose, structure[idx].homogeneous(), true); + Vec2 diff = observations[idx] - est; + + errorMax = std::max(errorMax, diff.norm()); + } + + return true; } } // namespace sfm -} // namespace aliceVision \ No newline at end of file +} // namespace aliceVision diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp index c7185694b9..1de7874d61 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp @@ -32,6 +32,7 @@ class SfmResection * @param viewId the view id to process * @param updatedPose output estimated pose * @param updatedThreshold estimated threshold + * @param inliersCount number of inliers for this resection * @return false if a critical error occured */ bool processView( @@ -41,7 +42,8 @@ class SfmResection std::mt19937 &randomNumberGenerator, const IndexT viewId, Eigen::Matrix4d & updatedPose, - double & updatedThreshold + double & updatedThreshold, + size_t & inliersCount ); private: @@ -61,7 +63,8 @@ class SfmResection const std::vector & observations, const std::vector & inliers, Eigen::Matrix4d & pose, - std::shared_ptr & intrinsics + std::shared_ptr & intrinsics, + double & errorMax ); private: diff --git a/src/aliceVision/sfm/pipeline/relativePoses.hpp b/src/aliceVision/sfm/pipeline/relativePoses.hpp index 62fcc6194b..e3a3c7fcc3 100644 --- a/src/aliceVision/sfm/pipeline/relativePoses.hpp +++ b/src/aliceVision/sfm/pipeline/relativePoses.hpp @@ -3,6 +3,7 @@ // 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/. +#pragma once #include #include diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp index bbe74e1a23..ba6b14aaf3 100644 --- a/src/aliceVision/sfm/sfmFilters.cpp +++ b/src/aliceVision/sfm/sfmFilters.cpp @@ -280,5 +280,46 @@ bool eraseUnstablePosesAndObservations(sfmData::SfMData& sfmData, return removedPoses || removedObservations; } +IndexT removeConstraints(sfmData::SfMData& sfmData, double maxDist) +{ + const auto & landmarks = sfmData.getLandmarks(); + auto & constraints = sfmData.getConstraintsPoint(); + + // Remove all constraints which are very far from associated landmark + + size_t count = 0; + auto itConstraints = constraints.begin(); + while (itConstraints != constraints.end()) + { + IndexT trackId = itConstraints->first; + + auto landmarkIt = landmarks.find(trackId); + + //If the associated landmark does not exists anymore, remove the constraint + if (landmarkIt == landmarks.end()) + { + itConstraints = constraints.erase(itConstraints); + count++; + continue; + } + + const Vec3 & lpt = landmarkIt->second.X; + double dist = (itConstraints->second.point - lpt).norm(); + + //Remove if the landmark is too far away + if (dist > maxDist) + { + itConstraints = constraints.erase(itConstraints); + count++; + } + else + { + ++itConstraints; + } + } + + return count; +} + } // namespace sfm } // namespace aliceVision diff --git a/src/aliceVision/sfm/sfmFilters.hpp b/src/aliceVision/sfm/sfmFilters.hpp index a2da677b2a..f878734a59 100644 --- a/src/aliceVision/sfm/sfmFilters.hpp +++ b/src/aliceVision/sfm/sfmFilters.hpp @@ -42,6 +42,14 @@ IndexT removeOutliersWithPixelResidualError(sfmData::SfMData& sfmData, // Return the number of removed tracks IndexT removeOutliersWithAngleError(sfmData::SfMData& sfmData, const double dMinAcceptedAngle); +/** + * @brief remove all point constraints which are too far away from their associated landmark + * @param sfmData the sfmData to update + * @param maxDist the maximal allowed distance between the landmark and the constraint + * @return the number of constraints removed + */ +IndexT removeConstraints(sfmData::SfMData& sfmData, const double maxDist); + bool eraseUnstablePoses(sfmData::SfMData& sfmData, const IndexT minPointsPerPose, std::set* outRemovedViewsId = NULL); bool eraseObservationsWithMissingPoses(sfmData::SfMData& sfmData, const IndexT minPointsPerLandmark); diff --git a/src/aliceVision/sfmData/CMakeLists.txt b/src/aliceVision/sfmData/CMakeLists.txt index 8815dad4cf..3d3d1f4051 100644 --- a/src/aliceVision/sfmData/CMakeLists.txt +++ b/src/aliceVision/sfmData/CMakeLists.txt @@ -11,6 +11,8 @@ set(sfmData_files_headers ImageInfo.hpp ExposureSetting.hpp Observation.hpp + ConstraintPoint.hpp + Constraint2D.hpp ) # Sources diff --git a/src/aliceVision/sfmData/ConstraintPoint.hpp b/src/aliceVision/sfmData/ConstraintPoint.hpp new file mode 100644 index 0000000000..97060ca28a --- /dev/null +++ b/src/aliceVision/sfmData/ConstraintPoint.hpp @@ -0,0 +1,40 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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/. + +#pragma once + +#include + +namespace aliceVision { +namespace sfmData { + +/** + * @brief ConstraintPoint defines a 3D constraint on a landmark + */ +struct ConstraintPoint +{ + ConstraintPoint() = default; + + ConstraintPoint(const IndexT paramLandmarkId, const Vec3 & paramNormal, const Vec3 & paramPoint) + : landmarkId(paramLandmarkId), + normal(paramNormal), + point(paramPoint) + {} + + IndexT landmarkId; + Vec3 normal; + Vec3 point; + + bool operator==(const ConstraintPoint& other) const + { + return (normal == other.normal) && (point == other.point) && (landmarkId == other.landmarkId); + } + + inline bool operator!=(const ConstraintPoint& other) const { return !(*this == other); } +}; + +} // namespace sfmData +} // namespace aliceVision diff --git a/src/aliceVision/sfmData/ConstraintPoint.i b/src/aliceVision/sfmData/ConstraintPoint.i new file mode 100644 index 0000000000..010ad46567 --- /dev/null +++ b/src/aliceVision/sfmData/ConstraintPoint.i @@ -0,0 +1,11 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 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 +%} \ No newline at end of file diff --git a/src/aliceVision/sfmData/SfMData.hpp b/src/aliceVision/sfmData/SfMData.hpp index 5af2392a01..57c49ab9ee 100644 --- a/src/aliceVision/sfmData/SfMData.hpp +++ b/src/aliceVision/sfmData/SfMData.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,9 @@ using LandmarksUncertainty = std::map; /// Define a collection of constraints using Constraints2D = std::vector; +/// Define a collection of constraints +using ConstraintsPoint = std::map; + /// Define a collection of rotation priors using RotationPriors = std::vector; @@ -66,6 +70,8 @@ class SfMData LandmarksUncertainty _landmarksUncertainty; /// 2D Constraints Constraints2D constraints2d; + /// Point constraints + ConstraintsPoint constraintsPoint; /// Rotation priors RotationPriors rotationpriors; @@ -134,6 +140,13 @@ class SfMData const Constraints2D& getConstraints2D() const { return constraints2d; } Constraints2D& getConstraints2D() { return constraints2d; } + /** + * @brief Get ConstraintsPoints + * @return ConstraintsPoints + */ + const ConstraintsPoint& getConstraintsPoint() const { return constraintsPoint; } + ConstraintsPoint& getConstraintsPoint() { return constraintsPoint; } + /** * @brief Get RotationPriors * @return RotationPriors @@ -247,6 +260,20 @@ class SfMData return nullptr; } + /** + * @brief Gives the intrinsic of the input intrinsic id. + * @param[in] intrinsicId The given intrinsic ID + * @return the corresponding intrinsic reference + */ + const camera::IntrinsicBase& getIntrinsic(IndexT intrinsicId) const { return *(_intrinsics.at(intrinsicId)); } + + /** + * @brief Gives the intrinsic of the input intrinsic id. + * @param[in] intrinsicId The given intrinsic ID + * @return the corresponding intrinsic reference + */ + camera::IntrinsicBase& getIntrinsic(IndexT intrinsicId) { return *(_intrinsics.at(intrinsicId)); } + /** * @brief Get a set of views keys * @return set of views keys diff --git a/src/aliceVision/sfmData/SfMData.i b/src/aliceVision/sfmData/SfMData.i index b6e4d5b41e..4d1845ee04 100644 --- a/src/aliceVision/sfmData/SfMData.i +++ b/src/aliceVision/sfmData/SfMData.i @@ -13,6 +13,7 @@ %include %include +%include %include %include %include diff --git a/src/software/export/main_exportMatches.cpp b/src/software/export/main_exportMatches.cpp index dbc84d4e33..3716d1ca54 100644 --- a/src/software/export/main_exportMatches.cpp +++ b/src/software/export/main_exportMatches.cpp @@ -31,7 +31,7 @@ // These constants define the current software version. // They must be updated when the command line is changed. -#define ALICEVISION_SOFTWARE_VERSION_MAJOR 1 +#define ALICEVISION_SOFTWARE_VERSION_MAJOR 2 #define ALICEVISION_SOFTWARE_VERSION_MINOR 0 using namespace aliceVision; @@ -87,6 +87,8 @@ int aliceVision_main(int argc, char** argv) std::string outputFolder; std::vector featuresFolders; std::vector matchesFolders; + std::string filterA = ""; + std::string filterB = ""; // user optional parameters @@ -107,7 +109,9 @@ int aliceVision_main(int argc, char** argv) po::options_description optionalParams("Optional parameters"); optionalParams.add_options() ("describerTypes,d", po::value(&describerTypesName)->default_value(describerTypesName), - EImageDescriberType_informations().c_str()); + EImageDescriberType_informations().c_str()) + ("filterA", po::value(&filterA)->default_value(filterA), "Restrict one of the two views to be this.") + ("filterB", po::value(&filterB)->default_value(filterB), "Restrict one of the two views to be this."); // clang-format on CmdLine cmdline("AliceVision exportMatches"); @@ -118,6 +122,7 @@ int aliceVision_main(int argc, char** argv) return EXIT_FAILURE; } + //Make sure we have somewhere to write images if (outputFolder.empty()) { ALICEVISION_LOG_ERROR("It is an invalid output folder"); @@ -131,9 +136,35 @@ int aliceVision_main(int argc, char** argv) ALICEVISION_LOG_ERROR("The input SfMData file '" << sfmDataFilename << "' cannot be read."); return EXIT_FAILURE; } + + //Create filters from input parameters + IndexT indexFilterA = UndefinedIndexT; + IndexT indexFilterB = UndefinedIndexT; - // load SfM Scene regions + if (!filterA.empty() || !filterB.empty()) + { + if (!filterA.empty()) + { + indexFilterA = sfmData.findView(filterA); + if (indexFilterA == UndefinedIndexT) + { + ALICEVISION_LOG_ERROR("Could not find corresponding view for: " + filterA); + return EXIT_FAILURE; + } + } + + if (!filterB.empty()) + { + indexFilterB = sfmData.findView(filterB); + if (indexFilterB == UndefinedIndexT) + { + ALICEVISION_LOG_ERROR("Could not find corresponding view for: " + filterB); + return EXIT_FAILURE; + } + } + } + // load SfM Scene regions // get imageDescriberMethodType std::vector describerMethodTypes = EImageDescriberType_stringToEnums(describerTypesName); @@ -164,21 +195,40 @@ int aliceVision_main(int argc, char** argv) const std::size_t I = iter->first; const std::size_t J = iter->second; - const View* viewI = sfmData.getViews().at(I).get(); - const View* viewJ = sfmData.getViews().at(J).get(); - const std::string viewImagePathI = viewI->getImage().getImagePath(); - const std::string viewImagePathJ = viewJ->getImage().getImagePath(); + //Apply filter A + if (indexFilterA != UndefinedIndexT) + { + if (I != indexFilterA && J != indexFilterA) + { + continue; + } + } + + //Apply filter B + if (indexFilterB != UndefinedIndexT) + { + if (I != indexFilterB && J != indexFilterB) + { + continue; + } + } + + const View & viewI = sfmData.getView(I); + const View & viewJ = sfmData.getView(J); + + const std::string viewImagePathI = viewI.getImage().getImagePath(); + const std::string viewImagePathJ = viewJ.getImage().getImagePath(); - std::string destFilename_I; - std::string destFilename_J; + std::string destFilenameI; + std::string destFilenameJ; { fs::path origImgPath(viewImagePathI); std::string origFilename = origImgPath.stem().string(); image::Image originalImage; image::readImage(viewImagePathI, originalImage, image::EImageColorSpace::LINEAR); - destFilename_I = (fs::path(outputFolder) / (origFilename + ".png")).string(); - image::writeImage(destFilename_I, originalImage, image::ImageWriteOptions().toColorSpace(image::EImageColorSpace::SRGB)); + destFilenameI = (fs::path(outputFolder) / (origFilename + ".png")).string(); + image::writeImage(destFilenameI, originalImage, image::ImageWriteOptions().toColorSpace(image::EImageColorSpace::SRGB)); } { @@ -186,25 +236,30 @@ int aliceVision_main(int argc, char** argv) std::string origFilename = origImgPath.stem().string(); image::Image originalImage; image::readImage(viewImagePathJ, originalImage, image::EImageColorSpace::LINEAR); - destFilename_J = (fs::path(outputFolder) / (origFilename + ".png")).string(); - image::writeImage(destFilename_J, originalImage, image::ImageWriteOptions().toColorSpace(image::EImageColorSpace::SRGB)); + destFilenameJ = (fs::path(outputFolder) / (origFilename + ".png")).string(); + image::writeImage(destFilenameJ, originalImage, image::ImageWriteOptions().toColorSpace(image::EImageColorSpace::SRGB)); } - const std::pair dimImageI = std::make_pair(viewI->getImage().getWidth(), viewI->getImage().getHeight()); - const std::pair dimImageJ = std::make_pair(viewJ->getImage().getWidth(), viewJ->getImage().getHeight()); + const std::pair dimImageI = std::make_pair(viewI.getImage().getWidth(), viewI.getImage().getHeight()); + const std::pair dimImageJ = std::make_pair(viewJ.getImage().getWidth(), viewJ.getImage().getHeight()); svgDrawer svgStream(dimImageI.first + dimImageJ.first, std::max(dimImageI.second, dimImageJ.second)); - svgStream.drawImage(destFilename_I, dimImageI.first, dimImageI.second); - svgStream.drawImage(destFilename_J, dimImageJ.first, dimImageJ.second, dimImageI.first); + svgStream.drawImage(destFilenameI, dimImageI.first, dimImageI.second); + svgStream.drawImage(destFilenameJ, dimImageJ.first, dimImageJ.second, dimImageI.first); + + struct PairInfo + { + Vec2 first; + Vec2 second; + feature::EImageDescriberType descType; + }; + std::vector pairs; const matching::MatchesPerDescType& filteredMatches = pairwiseMatches.at(*iter); ALICEVISION_LOG_INFO("nb describer: " << filteredMatches.size()); - if (filteredMatches.empty()) - continue; - for (const auto& matchesIt : filteredMatches) { const feature::EImageDescriberType descType = matchesIt.first; @@ -212,8 +267,8 @@ int aliceVision_main(int argc, char** argv) const matching::IndMatches& matches = matchesIt.second; ALICEVISION_LOG_INFO(EImageDescriberType_enumToString(matchesIt.first) << ": " << matches.size() << " matches"); - const PointFeatures& featuresI = featuresPerView.getFeatures(viewI->getViewId(), descType); - const PointFeatures& featuresJ = featuresPerView.getFeatures(viewJ->getViewId(), descType); + const PointFeatures& featuresI = featuresPerView.getFeatures(I, descType); + const PointFeatures& featuresJ = featuresPerView.getFeatures(J, descType); // draw link between features : for (std::size_t i = 0; i < matches.size(); ++i) @@ -221,30 +276,45 @@ int aliceVision_main(int argc, char** argv) const PointFeature& imaA = featuresI[matches[i]._i]; const PointFeature& imaB = featuresJ[matches[i]._j]; - // compute a flashy colour for the correspondence - unsigned char r, g, b; - hslToRgb((rand() % 360) / 360., 1.0, .5, r, g, b); - std::ostringstream osCol; - osCol << "rgb(" << (int)r << ',' << (int)g << ',' << (int)b << ")"; - svgStream.drawLine(imaA.x(), imaA.y(), imaB.x() + dimImageI.first, imaB.y(), svgStyle().stroke(osCol.str(), 2.0)); - } + PairInfo pi; - const std::string featColor = describerTypeColor(descType); - // draw features (in two loop, in order to have the features upper the link, svg layer order): - for (std::size_t i = 0; i < matches.size(); ++i) - { - const PointFeature& imaA = featuresI[matches[i]._i]; - const PointFeature& imaB = featuresJ[matches[i]._j]; - svgStream.drawCircle(imaA.x(), imaA.y(), 5.0, svgStyle().stroke(featColor, 2.0)); - svgStream.drawCircle(imaB.x() + dimImageI.first, imaB.y(), 5.0, svgStyle().stroke(featColor, 2.0)); + pi.first = imaA.coords().cast(); + pi.second = imaB.coords().cast(); + pi.descType = descType; + + pairs.push_back(pi); } } - fs::path outputFilename = fs::path(outputFolder) / std::string(std::to_string(iter->first) + "_" + std::to_string(iter->second) + "_" + - std::to_string(filteredMatches.getNbAllMatches()) + ".svg"); + if (pairs.empty()) + { + continue; + } + + for (const auto & pi : pairs) + { + // compute a flashy colour for the correspondence + unsigned char r, g, b; + hslToRgb((rand() % 360) / 360., 1.0, .5, r, g, b); + std::ostringstream osCol; + osCol << "rgb(" << (int)r << ',' << (int)g << ',' << (int)b << ")"; + svgStream.drawLine(pi.first.x(), pi.first.y(), pi.second.x() + dimImageI.first, pi.second.y(), svgStyle().stroke(osCol.str(), 2.0)); + } + + // draw features (in two loop, in order to have the features upper the link, svg layer order): + for (const auto & pi : pairs) + { + const std::string featColor = describerTypeColor(pi.descType); + svgStream.drawCircle(pi.first.x(), pi.first.y(), 5.0, svgStyle().stroke(featColor, 2.0)); + svgStream.drawCircle(pi.second.x() + dimImageI.first, pi.second.y(), 5.0, svgStyle().stroke(featColor, 2.0)); + } + + fs::path outputFilename = fs::path(outputFolder) / std::string(std::to_string(iter->first) + "_" + std::to_string(iter->second) + "_" +std::to_string(filteredMatches.getNbAllMatches()) + ".svg"); std::ofstream svgFile(outputFilename.string()); svgFile << svgStream.closeSvgFile().str(); svgFile.close(); } + + return EXIT_SUCCESS; } diff --git a/src/software/pipeline/CMakeLists.txt b/src/software/pipeline/CMakeLists.txt index 7914294b28..48d7092836 100644 --- a/src/software/pipeline/CMakeLists.txt +++ b/src/software/pipeline/CMakeLists.txt @@ -129,6 +129,7 @@ if(ALICEVISION_BUILD_SFM) aliceVision_sfmData aliceVision_track aliceVision_dataio + aliceVision_mesh Boost::program_options Boost::json ) @@ -144,6 +145,7 @@ if(ALICEVISION_BUILD_SFM) aliceVision_sfmData aliceVision_track aliceVision_dataio + aliceVision_mesh Boost::program_options Boost::json ) diff --git a/src/software/pipeline/main_relativePoseEstimating.cpp b/src/software/pipeline/main_relativePoseEstimating.cpp index a5f0fdbf7a..d94b277271 100644 --- a/src/software/pipeline/main_relativePoseEstimating.cpp +++ b/src/software/pipeline/main_relativePoseEstimating.cpp @@ -40,7 +40,7 @@ // These constants define the current software version. // They must be updated when the command line is changed. -#define ALICEVISION_SOFTWARE_VERSION_MAJOR 2 +#define ALICEVISION_SOFTWARE_VERSION_MAJOR 3 #define ALICEVISION_SOFTWARE_VERSION_MINOR 0 using namespace aliceVision; @@ -140,8 +140,9 @@ int aliceVision_main(int argc, char** argv) std::string outputDirectory; int rangeStart = -1; int rangeSize = 1; - const size_t minInliers = 35; + size_t minInliers = 35; bool enforcePureRotation = false; + size_t countIterations = 1024; // user optional parameters std::string describerTypesName = feature::EImageDescriberType_enumToString(feature::EImageDescriberType::SIFT); @@ -158,6 +159,8 @@ int aliceVision_main(int argc, char** argv) po::options_description optionalParams("Optional parameters"); optionalParams.add_options() ("enforcePureRotation,e", po::value(&enforcePureRotation)->default_value(enforcePureRotation), "Enforce pure rotation in estimation.") + ("countIterations", po::value(&countIterations)->default_value(countIterations), "Maximal number of iterations.") + ("minInliers", po::value(&minInliers)->default_value(minInliers), "Minimal number of inliers for a valid ransac.") ("rangeStart", po::value(&rangeStart)->default_value(rangeStart), "Range image index start.") ("rangeSize", po::value(&rangeSize)->default_value(rangeSize), "Range size."); // clang-format on @@ -285,7 +288,7 @@ int aliceVision_main(int argc, char** argv) refpts, nextpts, randomNumberGenerator, - 1024, + countIterations, minInliers); if (!relativeSuccess) { @@ -308,7 +311,7 @@ int aliceVision_main(int argc, char** argv) refpts, nextpts, randomNumberGenerator, - 1024, + countIterations, minInliers); if (!essentialSuccess) { @@ -327,6 +330,11 @@ int aliceVision_main(int argc, char** argv) continue; } + if (vecInliers.size() < minInliers) + { + continue; + } + reconstructed.pose = geometry::Pose3(T); } diff --git a/src/software/pipeline/main_sfmBootstraping.cpp b/src/software/pipeline/main_sfmBootstraping.cpp index 980c299221..a5b18a3517 100644 --- a/src/software/pipeline/main_sfmBootstraping.cpp +++ b/src/software/pipeline/main_sfmBootstraping.cpp @@ -26,18 +26,18 @@ #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. -#define ALICEVISION_SOFTWARE_VERSION_MAJOR 2 +#define ALICEVISION_SOFTWARE_VERSION_MAJOR 3 #define ALICEVISION_SOFTWARE_VERSION_MINOR 0 using namespace aliceVision; @@ -46,165 +46,58 @@ namespace po = boost::program_options; namespace fs = boost::filesystem; /** - * @brief estimate a median angle (parallax) between a reference view and another view - * @param sfmData the input sfmData which contains camera information + * @brief build an initial set of landmarks from a view and a mesh object + * @param sfmData the input/output sfmData + * @param meshFilename the mesh path * @param referenceViewId the reference view id - * @param otherViewId the other view id - * @param otherTreference the relative pose * @param tracksMap the input map of tracks - * @param tracksPerView tracks grouped by views - * @param resultAngle the output median angle - * @param usedTracks the list of tracks which were succesfully reconstructed * @return true */ -bool estimatePairAngle(const sfmData::SfMData & sfmData, - const IndexT referenceViewId, - const IndexT otherViewId, - const geometry::Pose3 & otherTreference, - const track::TracksMap& tracksMap, - const track::TracksPerView & tracksPerView, - double & resultAngle, - std::vector & usedTracks) +bool landmarksFromMesh( + sfmData::Landmarks & landmarks, + const sfmData::SfMData & sfmData, + const std::string & meshFilename, + const IndexT referenceViewId, + const track::TracksHandler& tracksHandler) { - usedTracks.clear(); - - const sfmData::View& refView = sfmData.getView(referenceViewId); - const sfmData::View& nextView = sfmData.getView(otherViewId); - - std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); - std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); - - aliceVision::track::TracksMap mapTracksCommon; - track::getCommonTracksInImagesFast({referenceViewId, otherViewId}, tracksMap, tracksPerView, mapTracksCommon); - - const Mat4 T1 = Eigen::Matrix4d::Identity(); - const Mat4 T2 = otherTreference.getHomogeneous(); - - const Eigen::Vector3d c = otherTreference.center(); - - size_t count = 0; - std::vector angles; - for(const auto& commonItem : mapTracksCommon) + //Load mesh in the mesh intersection object + ALICEVISION_LOG_INFO("Loading mesh"); + mesh::MeshIntersection mi; + if (!mi.initialize(meshFilename)) { - const track::Track& track = commonItem.second; - - const IndexT refFeatureId = track.featPerView.at(referenceViewId).featureId; - const IndexT nextfeatureId = track.featPerView.at(otherViewId).featureId; - - const Vec2 refpt = track.featPerView.at(referenceViewId).coords; - const Vec2 nextpt = track.featPerView.at(otherViewId).coords; - - const Vec3 pt3d1 = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refpt))); - const Vec3 pt3d2 = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextpt))); - - - Vec3 X; - multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X); - - //Make sure - Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); - Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); - if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) - { - continue; - } - - const Vec3 ray1 = - X; - const Vec3 ray2 = c - X; - const double cangle = clamp(ray1.normalized().dot(ray2.normalized()), -1.0, 1.0); - const double angle = std::acos(cangle); - angles.push_back(angle); - - usedTracks.push_back(commonItem.first); + return EXIT_FAILURE; } - if (angles.size() == 0) - { - resultAngle = 0.0; - return false; - } + const sfmData::View & v = sfmData.getView(referenceViewId); + const sfmData::CameraPose & cpose = sfmData.getAbsolutePose(v.getPoseId()); + const camera::IntrinsicBase & intrinsic = sfmData.getIntrinsic(v.getIntrinsicId()); - const unsigned medianIndex = angles.size() / 2; - std::nth_element(angles.begin(), angles.begin() + medianIndex, angles.end()); - resultAngle = angles[medianIndex]; - return true; -} + mi.setPose(cpose.getTransform()); + const auto & trackIds = tracksHandler.getTracksPerView().at(referenceViewId); + const auto & tracksMap = tracksHandler.getAllTracks(); -/** - * @brief build an initial sfmData from two views - * @param sfmData the input/output sfmData - * @param referenceViewId the reference view id - * @param otherViewId the other view id - * @param otherTreference the relative pose - * @param tracksMap the input map of tracks - * @param usedTracks the input list of valid tracks - * @return true -*/ -bool buildSfmData(sfmData::SfMData & sfmData, - const IndexT referenceViewId, - const IndexT otherViewId, - const geometry::Pose3 & otherTreference, - const track::TracksMap& tracksMap, - const std::vector & usedTracks) -{ - const sfmData::View& refView = sfmData.getView(referenceViewId); - const sfmData::View& nextView = sfmData.getView(otherViewId); - - std::shared_ptr refIntrinsics = sfmData.getIntrinsicSharedPtr(refView.getIntrinsicId()); - std::shared_ptr nextIntrinsics = sfmData.getIntrinsicSharedPtr(nextView.getIntrinsicId()); - - sfmData::CameraPose cposeNext(otherTreference, false); - sfmData.getPoses()[refView.getPoseId()] = sfmData::CameraPose(); - sfmData.getPoses()[nextView.getPoseId()] = cposeNext; - - const Mat4 T1 = Eigen::Matrix4d::Identity(); - Mat4 T2 = otherTreference.getHomogeneous(); - - size_t count = 0; - std::vector angles; - for(const auto& trackId : usedTracks) + for (const auto trackId : trackIds) { - const track::Track& track = tracksMap.at(trackId); - + const track::Track & track = tracksMap.at(trackId); const track::TrackItem & refItem = track.featPerView.at(referenceViewId); - const track::TrackItem & nextItem = track.featPerView.at(otherViewId); - - const Vec2 refpt = track.featPerView.at(referenceViewId).coords; - const Vec2 nextpt = track.featPerView.at(otherViewId).coords; - const Vec3 pt3d1 = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refpt))); - const Vec3 pt3d2 = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextpt))); - + const Vec2 refpt = track.featPerView.at(referenceViewId).coords; + const std::size_t featureId = track.featPerView.at(referenceViewId).featureId; + const double scale = track.featPerView.at(referenceViewId).scale; - Vec3 X; - multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X); - - Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized(); - Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized(); - if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2) > 0.0)) + Vec3 point; + if (!mi.pickPoint(point, intrinsic, refpt)) { continue; } - sfmData::Landmark landmark; - landmark.descType = track.descType; - landmark.X = X; - - sfmData::Observation refObs; - refObs.setFeatureId(refItem.featureId); - refObs.setScale(refItem.scale); - refObs.setCoordinates(refItem.coords); - - sfmData::Observation nextObs; - nextObs.setFeatureId(nextItem.featureId); - nextObs.setScale(nextItem.scale); - nextObs.setCoordinates(nextItem.coords); - - landmark.getObservations()[referenceViewId] = refObs; - landmark.getObservations()[otherViewId] = nextObs; - - sfmData.getLandmarks()[trackId] = landmark; + sfmData::Landmark l; + l.X = point; + l.descType = feature::EImageDescriberType::SIFT; + sfmData::Observations & observations = l.getObservations(); + observations[referenceViewId] = sfmData::Observation(refpt, featureId, scale); + landmarks[trackId] = l; } return true; @@ -216,6 +109,7 @@ int aliceVision_main(int argc, char** argv) std::string sfmDataFilename; std::string sfmDataOutputFilename; std::string tracksFilename; + std::string meshFilename; std::string pairsDirectory; // user optional parameters @@ -238,6 +132,7 @@ int aliceVision_main(int argc, char** argv) optionalParams.add_options() ("minAngleInitialPair", po::value(&minAngle)->default_value(minAngle), "Minimum angle for the initial pair.") ("maxAngleInitialPair", po::value(&maxAngle)->default_value(maxAngle), "Maximum angle for the initial pair.") + ("meshFilename,t", po::value(&meshFilename)->required(), "Mesh object file.") ("initialPairA", po::value(&initialPairString.first)->default_value(initialPairString.first), "UID or filepath or filename of the first image.") ("initialPairB", po::value(&initialPairString.second)->default_value(initialPairString.second), "UID or filepath or filename of the second image."); @@ -269,6 +164,13 @@ int aliceVision_main(int argc, char** argv) return EXIT_SUCCESS; } + if (sfmData.getValidViews().size() == 1) + { + ALICEVISION_LOG_INFO("SfmData has one view with a pose. Assuming we want to use it."); + initialPairString.first = std::to_string(*sfmData.getValidViews().begin()); + } + + if (!initialPairString.first.empty() || !initialPairString.second.empty()) { if (initialPairString.first == initialPairString.second) @@ -318,6 +220,16 @@ int aliceVision_main(int argc, char** argv) } + //Load mesh in the mesh intersection object + bool useMesh = false; + sfmData::Landmarks landmarks; + if (!meshFilename.empty() && initialPair.first != UndefinedIndexT) + { + landmarksFromMesh(landmarks, sfmData, meshFilename, initialPair.first, tracksHandler); + + useMesh = true; + } + //Result of pair estimations are stored in multiple files std::vector reconstructedPairs; const std::regex regex("pairs\\_[0-9]+\\.json"); @@ -335,11 +247,32 @@ int aliceVision_main(int argc, char** argv) for (const boost::json::value & value : values) { std::vector localVector = boost::json::value_to>(value); - reconstructedPairs.insert(reconstructedPairs.end(), localVector.begin(), localVector.end()); + + for (const auto & pair: localVector) + { + // Filter out pairs given user filters + if (initialPair.first != UndefinedIndexT) + { + if (pair.reference != initialPair.first && pair.next != initialPair.first) + { + continue; + } + } + + // Filter out pairs given user filters + if (initialPair.second != UndefinedIndexT) + { + if (pair.reference != initialPair.second && pair.next != initialPair.second) + { + continue; + } + } + + reconstructedPairs.push_back(pair); + } } } - //Check all pairs ALICEVISION_LOG_INFO("Give a score to all pairs"); int count = 0; @@ -349,79 +282,40 @@ int aliceVision_main(int argc, char** argv) bestPair.reference = UndefinedIndexT; std::vector bestUsedTracks; - for (const sfm::ReconstructedPair & pair: reconstructedPairs) - { - std::vector usedTracks; - double angle = 0.0; - - if (initialPair.first != UndefinedIndexT) - { - if (pair.reference != initialPair.first && pair.next != initialPair.first) - { - continue; - } - } - - if (initialPair.second != UndefinedIndexT) - { - if (pair.reference != initialPair.second && pair.next != initialPair.second) - { - continue; - } - } - - ALICEVISION_LOG_INFO("Processing pair " << initialPair.first << " / " << initialPair.second); - - if (!estimatePairAngle(sfmData, pair.reference, pair.next, pair.pose, tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), angle, usedTracks)) - { - continue; - } - - ALICEVISION_LOG_INFO("angle " << initialPair.first << " / " << initialPair.second << " : " << radianToDegree(angle)); - - if (radianToDegree(angle) > maxAngle) - { - continue; - } - - //If the angle is too small, then dramatically reduce its chances - if (radianToDegree(angle) < minAngle) - { - angle = -1.0 / angle; - } - - const sfmData::View & vref = sfmData.getView(pair.reference); - const sfmData::View & vnext = sfmData.getView(pair.next); - - int maxref = std::max(vref.getImage().getWidth(), vref.getImage().getHeight()); - int maxnext = std::max(vnext.getImage().getWidth(), vnext.getImage().getHeight()); - - double refScore = sfm::ExpansionPolicyLegacy::computeScore(tracksHandler.getAllTracks(), usedTracks, pair.reference, maxref, 5); - double nextScore = sfm::ExpansionPolicyLegacy::computeScore(tracksHandler.getAllTracks(), usedTracks, pair.next, maxnext, 5); + IndexT bestPairId = findBestPair(sfmData, reconstructedPairs, + tracksHandler.getAllTracks(), tracksHandler.getTracksPerView(), + minAngle, maxAngle); - ALICEVISION_LOG_INFO("image score " << initialPair.first << " / " << initialPair.second << " : " << refScore << " / " << nextScore); - - double score = std::min(refScore, nextScore) * std::max(1.0, radianToDegree(angle)); - - - if (score > bestScore) - { - bestPair = pair; - bestScore = score; - bestUsedTracks = usedTracks; - } - } - - if (bestPair.reference == UndefinedIndexT) + if (bestPairId == UndefinedIndexT) { ALICEVISION_LOG_INFO("No valid pair"); return EXIT_FAILURE; } + + bestPair = reconstructedPairs[bestPairId]; - if (!buildSfmData(sfmData, bestPair.reference, bestPair.next, bestPair.pose, tracksHandler.getAllTracks(), bestUsedTracks)) + if (useMesh) { - return EXIT_FAILURE; + if (!sfm::bootstrapMesh(sfmData, + landmarks, + bestPair.reference, bestPair.next, + tracksHandler.getAllTracks(), tracksHandler.getTracksPerView())) + { + return EXIT_FAILURE; + } } + else + { + if (!sfm::bootstrapBase(sfmData, + bestPair.reference, bestPair.next, + bestPair.pose, + tracksHandler.getAllTracks(), tracksHandler.getTracksPerView())) + { + return EXIT_FAILURE; + } + } + + std::cout << sfmData.getLandmarks().size() << std::endl; ALICEVISION_LOG_INFO("Best selected pair is : "); ALICEVISION_LOG_INFO(" - " << sfmData.getView(bestPair.reference).getImage().getImagePath()); diff --git a/src/software/pipeline/main_sfmExpanding.cpp b/src/software/pipeline/main_sfmExpanding.cpp index 7463a87517..9ffd1d717f 100644 --- a/src/software/pipeline/main_sfmExpanding.cpp +++ b/src/software/pipeline/main_sfmExpanding.cpp @@ -20,23 +20,69 @@ #include #include +#include + #include // These constants define the current software version. // They must be updated when the command line is changed. -#define ALICEVISION_SOFTWARE_VERSION_MAJOR 1 +#define ALICEVISION_SOFTWARE_VERSION_MAJOR 2 #define ALICEVISION_SOFTWARE_VERSION_MINOR 0 using namespace aliceVision; namespace po = boost::program_options; +//This intermediate class is used as a proxy to not link +//sfm with mesh library +class MeshPointFetcher : public sfm::PointFetcher +{ +public: + /** + * @brief initialize object. to be called before any other method + * @param pathToModel path to obj file to use as mesh + */ + bool initialize(const std::string & pathToModel) + { + return _mi.initialize(pathToModel); + } + + /** + * @brief Set the pose of the camera + * @param pose the pose of the camera wrt some global coordinates frame + */ + void setPose(const geometry::Pose3 & pose) override + { + _mi.setPose(pose); + } + + /** + * @brief virtual method to get coordinates and normals of a pixel of an image + * @param point result point in some global coordinates frame + * @param normal result normal in some global coordinates frame + * @param intrinsic the camera intrinsic object + * @param imageCoords the input image pixel coordinates in 2D. + * @return false on error + */ + bool pickPointAndNormal(Vec3 & point, + Vec3 & normal, + const camera::IntrinsicBase & intrinsic, + const Vec2 & imageCoords) override + { + return _mi.pickPointAndNormal(point, normal, intrinsic, imageCoords); + } + +private: + mesh::MeshIntersection _mi; +}; + int aliceVision_main(int argc, char** argv) { // command-line parameters std::string sfmDataFilename; std::string sfmDataOutputFilename; std::string tracksFilename; + std::string meshFilename; std::size_t localizerEstimatorMaxIterations = 50000; double localizerEstimatorError = 0.0; @@ -93,6 +139,7 @@ int aliceVision_main(int argc, char** argv) "If minNbCamerasToRefinePrincipalPoint==1, the principal point is always refined.") ("useRigConstraint", po::value(&useRigConstraint)->default_value(useRigConstraint), "Enable/Disable rig constraint.") ("rigMinNbCamerasForCalibration", po::value(&minNbCamerasForRigCalibration)->default_value(minNbCamerasForRigCalibration),"Minimal number of cameras to start the calibration of the rig.") + ("meshFilename,t", po::value(&meshFilename)->default_value(meshFilename), "Mesh file."); ; // clang-format on @@ -173,6 +220,20 @@ int aliceVision_main(int argc, char** argv) sfmBundle->setMaxReprojectionError(maxReprojectionError); sfmBundle->setMinNbCamerasToRefinePrincipalPoint(minNbCamerasToRefinePrincipalPoint); + sfm::PointFetcher::uptr pointFetcherHandler; + if (!meshFilename.empty()) + { + ALICEVISION_LOG_INFO("Load mesh"); + std::unique_ptr handler = std::make_unique(); + + if (!handler->initialize(meshFilename)) + { + return EXIT_FAILURE; + } + + pointFetcherHandler = std::move(handler); + } + sfm::ExpansionChunk::uptr expansionChunk = std::make_unique(); expansionChunk->setBundleHandler(sfmBundle); expansionChunk->setExpansionHistoryHandler(expansionHistory); @@ -180,6 +241,7 @@ int aliceVision_main(int argc, char** argv) expansionChunk->setResectionMaxError(localizerEstimatorError); expansionChunk->setTriangulationMinPoints(minNbObservationsForTriangulation); expansionChunk->setMinAngleTriangulation(minAngleForTriangulation); + expansionChunk->setPointFetcherHandler(pointFetcherHandler); sfm::ExpansionPolicy::uptr expansionPolicy; { diff --git a/src/software/utils/main_depthMapRendering.cpp b/src/software/utils/main_depthMapRendering.cpp index e47720a06b..4cc2eb2fdd 100644 --- a/src/software/utils/main_depthMapRendering.cpp +++ b/src/software/utils/main_depthMapRendering.cpp @@ -117,7 +117,7 @@ int aliceVision_main(int argc, char** argv) //Find the 3d point //Which is the intersection of the ray and the mesh Vec3 pt3d; - if (!mi.peekPoint(pt3d, *intrinsic, pt)) + if (!mi.pickPoint(pt3d, *intrinsic, pt)) { continue; } diff --git a/src/software/utils/main_normalMapRendering.cpp b/src/software/utils/main_normalMapRendering.cpp index 61c758586d..58b261a606 100644 --- a/src/software/utils/main_normalMapRendering.cpp +++ b/src/software/utils/main_normalMapRendering.cpp @@ -109,7 +109,7 @@ int aliceVision_main(int argc, char** argv) //Which is the intersection of the ray and the mesh //And get its normal Vec3 normal; - if (!mi.peekNormal(normal, *intrinsic, pt)) + if (!mi.pickNormal(normal, *intrinsic, pt)) { continue; } diff --git a/src/software/utils/main_sfmPoseInjecting.cpp b/src/software/utils/main_sfmPoseInjecting.cpp index b8fa0d5f87..1f887520af 100644 --- a/src/software/utils/main_sfmPoseInjecting.cpp +++ b/src/software/utils/main_sfmPoseInjecting.cpp @@ -110,9 +110,20 @@ bool getPoseFromJson(const boost::json::object& obj, ERotationFormat format, Pos t.y() = boost::json::value_to(obj.at("ty")); t.z() = boost::json::value_to(obj.at("tz")); - readPose.T = Eigen::Matrix4d::Identity(); - readPose.T.block<3, 3>(0, 0) = R; - readPose.T.block<3, 1>(0, 3) = t; + Eigen::Matrix4d world_T_camera = Eigen::Matrix4d::Identity(); + world_T_camera.block<3, 3>(0, 0) = R; + world_T_camera.block<3, 1>(0, 3) = t; + + //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; + + + world_T_camera = aliceTinput * world_T_camera * aliceTinput.inverse(); + readPose.T = world_T_camera.inverse(); return true; } @@ -223,4 +234,4 @@ int aliceVision_main(int argc, char** argv) sfmDataIO::save(sfmData, sfmDataOutputFilename, sfmDataIO::ESfMData::ALL); return EXIT_SUCCESS; -} \ No newline at end of file +} diff --git a/src/software/utils/main_sfmTransform.cpp b/src/software/utils/main_sfmTransform.cpp index 05e5d7b4f4..55f5b3886e 100644 --- a/src/software/utils/main_sfmTransform.cpp +++ b/src/software/utils/main_sfmTransform.cpp @@ -409,7 +409,7 @@ bool parseLineUp(const std::string & lineUpFilename, const std::string & tracksF const Vec2 & imageCoords = trackitem.coords; Vec3 pt3d; - if (!meshIntersection.peekPoint(pt3d, *intrinsic, imageCoords)) + if (!meshIntersection.pickPoint(pt3d, *intrinsic, imageCoords)) { continue; }