diff --git a/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp b/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp new file mode 100644 index 0000000000..c67bb519b7 --- /dev/null +++ b/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp @@ -0,0 +1,38 @@ +// 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 Vec3 & normal, const Vec3 & point) + : _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] = 100.0 * (distance - _constraintDistance); + + return true; + } + + Vec3 _normal; + double _constraintDistance; +}; + +} // namespace sfm +} // namespace aliceVision diff --git a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp index bc821e5b64..4c4d7caed3 100644 --- a/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp @@ -86,6 +86,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 +163,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->peekPointAndNormal(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.size() == 0) + { + 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..417fdfc284 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 { @@ -19,7 +20,7 @@ class ExpansionChunk { public: using uptr = std::unique_ptr; - + public: /** @@ -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; @@ -106,9 +116,18 @@ 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; private: size_t _resectionIterations = 1024; diff --git a/src/aliceVision/sfm/pipeline/expanding/PointFetcher.hpp b/src/aliceVision/sfm/pipeline/expanding/PointFetcher.hpp new file mode 100644 index 0000000000..59fca5035c --- /dev/null +++ b/src/aliceVision/sfm/pipeline/expanding/PointFetcher.hpp @@ -0,0 +1,45 @@ +// 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 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 pose pose of the camera wrt 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 peekPointAndNormal(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/SfmResection.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp index 60d8b9bd61..246aa93e57 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmResection.cpp @@ -91,11 +91,12 @@ bool SfmResection::processView( } //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 +141,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 +157,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 +181,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..20cb59a292 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmResection.hpp @@ -61,7 +61,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/software/pipeline/main_sfmExpanding.cpp b/src/software/pipeline/main_sfmExpanding.cpp index 7463a87517..4ac594c99b 100644 --- a/src/software/pipeline/main_sfmExpanding.cpp +++ b/src/software/pipeline/main_sfmExpanding.cpp @@ -20,6 +20,8 @@ #include #include +#include + #include // These constants define the current software version. @@ -31,12 +33,57 @@ 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); + } + + /** + * Set the pose of the camera + * @param 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 pose pose of the camera wrt some global coordinates frame + * @param intrinsic the camera intrinsic object + * @param imageCoords the input image pixel coordinates in 2D. + * @return false on error + */ + bool peekPointAndNormal(Vec3 & point, + Vec3 & normal, + const camera::IntrinsicBase & intrinsic, + const Vec2 & imageCoords) override + { + return _mi.peekPointAndNormal(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 +140,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 +221,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 +242,7 @@ int aliceVision_main(int argc, char** argv) expansionChunk->setResectionMaxError(localizerEstimatorError); expansionChunk->setTriangulationMinPoints(minNbObservationsForTriangulation); expansionChunk->setMinAngleTriangulation(minAngleForTriangulation); + expansionChunk->setPointFetcherHandler(pointFetcherHandler); sfm::ExpansionPolicy::uptr expansionPolicy; {