From 10aba3891954d9a1e189ace1da81e67238e888b4 Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Wed, 13 Dec 2023 10:10:44 +0100 Subject: [PATCH 1/3] replace observation structure with a class --- src/aliceVision/depthMap/SgmDepthList.cpp | 4 +- src/aliceVision/fuseCut/Fuser.cpp | 2 +- .../localization/CCTagLocalizer.cpp | 2 +- .../localization/VoctreeLocalizer.cpp | 2 +- src/aliceVision/mvsUtils/MultiViewParams.cpp | 6 +- .../photometricStereo/normalIntegration.cpp | 8 +- src/aliceVision/sfm/ResidualErrorFunctor.hpp | 54 +++++----- .../sfm/bundle/BundleAdjustmentCeres.cpp | 6 +- .../bundle/BundleAdjustmentSymbolicCeres.cpp | 8 +- .../sfm/bundle/bundleAdjustment_test.cpp | 2 +- .../sfm/bundle/costfunctions/projection.hpp | 6 +- .../bundle/costfunctions/projectionSimple.hpp | 6 +- src/aliceVision/sfm/generateReport.cpp | 2 +- .../sfm/pipeline/ReconstructionEngine.cpp | 4 +- src/aliceVision/sfm/pipeline/RigSequence.cpp | 2 +- .../ReconstructionEngine_panorama.cpp | 4 +- .../ReconstructionEngine_sequentialSfM.cpp | 2 +- src/aliceVision/sfm/sfmFilters.cpp | 8 +- src/aliceVision/sfm/sfmStatistics.cpp | 6 +- src/aliceVision/sfm/sfmTriangulation.cpp | 6 +- src/aliceVision/sfm/utils/statistics.cpp | 2 +- src/aliceVision/sfm/utils/syntheticScene.cpp | 2 +- src/aliceVision/sfm/utils/syntheticScene.hpp | 6 +- src/aliceVision/sfmData/CMakeLists.txt | 2 + src/aliceVision/sfmData/Landmark.hpp | 22 +---- src/aliceVision/sfmData/Observation.cpp | 18 ++++ src/aliceVision/sfmData/Observation.hpp | 98 +++++++++++++++++++ src/aliceVision/sfmData/colorize.cpp | 2 +- src/aliceVision/sfmDataIO/AlembicExporter.cpp | 8 +- src/aliceVision/sfmDataIO/AlembicImporter.cpp | 12 +-- src/aliceVision/sfmDataIO/bafIO.cpp | 2 +- src/aliceVision/sfmDataIO/colmap.cpp | 4 +- src/aliceVision/sfmDataIO/jsonIO.cpp | 12 +-- src/software/export/main_exportMVE2.cpp | 2 +- src/software/export/main_exportMatlab.cpp | 4 +- src/software/export/main_exportPMVS.cpp | 2 +- .../pipeline/main_sfmBootstraping.cpp | 12 +-- src/software/utils/main_sfmRegression.cpp | 6 +- 38 files changed, 224 insertions(+), 132 deletions(-) create mode 100644 src/aliceVision/sfmData/Observation.cpp create mode 100644 src/aliceVision/sfmData/Observation.hpp diff --git a/src/aliceVision/depthMap/SgmDepthList.cpp b/src/aliceVision/depthMap/SgmDepthList.cpp index 35a5fafa01..c898aae318 100644 --- a/src/aliceVision/depthMap/SgmDepthList.cpp +++ b/src/aliceVision/depthMap/SgmDepthList.cpp @@ -305,7 +305,7 @@ void SgmDepthList::getMinMaxMidNbDepthFromSfM(float& out_min, float& out_max, fl continue; // get rc 2d observation - const Vec2& obs2d = it->second.x; + const Vec2& obs2d = it->second.getCoordinates(); // if we compute depth list per tile keep only observation located inside the inflated image full-size ROI if (!_sgmParams.depthListPerTile || fullsizeRoi.contains(obs2d.x(), obs2d.y())) @@ -381,7 +381,7 @@ void SgmDepthList::getRcTcDepthRangeFromSfM(int tc, double& out_zmin, double& ou continue; // get rc 2d observation - const Vec2& obs2d = it->second.x; + const Vec2& obs2d = it->second.getCoordinates(); // observation located inside the inflated image full-size ROI if (!_sgmParams.depthListPerTile || fullsizeRoi.contains(obs2d.x(), obs2d.y())) diff --git a/src/aliceVision/fuseCut/Fuser.cpp b/src/aliceVision/fuseCut/Fuser.cpp index 26a0d3d9ff..a72fb979ab 100644 --- a/src/aliceVision/fuseCut/Fuser.cpp +++ b/src/aliceVision/fuseCut/Fuser.cpp @@ -526,7 +526,7 @@ bool checkLandmarkMinObservationAngle(const sfmData::SfMData& sfmData, const sfm const camera::IntrinsicBase* intrinsicPtrJ = sfmData.getIntrinsicPtr(viewJ.getIntrinsicId()); const double angle = - camera::angleBetweenRays(poseI, intrinsicPtrI, poseJ, intrinsicPtrJ, observationPairI.second.x, observationPairJ.second.x); + camera::angleBetweenRays(poseI, intrinsicPtrI, poseJ, intrinsicPtrJ, observationPairI.second.getCoordinates(), observationPairJ.second.getCoordinates()); // check angle between two observation if (angle < minObservationAngle) diff --git a/src/aliceVision/localization/CCTagLocalizer.cpp b/src/aliceVision/localization/CCTagLocalizer.cpp index 8ecfc2e0bf..c103ed1df8 100644 --- a/src/aliceVision/localization/CCTagLocalizer.cpp +++ b/src/aliceVision/localization/CCTagLocalizer.cpp @@ -81,7 +81,7 @@ bool CCTagLocalizer::loadReconstructionDescriptors(const sfmData::SfMData& sfm_d { const IndexT viewId = obs.first; const sfmData::Observation& obs2d = obs.second; - observationsPerView[viewId][landmark.descType].emplace_back(obs2d.id_feat, trackId); + observationsPerView[viewId][landmark.descType].emplace_back(obs2d.getFeatureId(), trackId); } } for (auto& featuresPerTypeInImage : observationsPerView) diff --git a/src/aliceVision/localization/VoctreeLocalizer.cpp b/src/aliceVision/localization/VoctreeLocalizer.cpp index 952a62e819..8139ebfe43 100644 --- a/src/aliceVision/localization/VoctreeLocalizer.cpp +++ b/src/aliceVision/localization/VoctreeLocalizer.cpp @@ -290,7 +290,7 @@ bool VoctreeLocalizer::initDatabase(const std::string& vocTreeFilepath, const st { const IndexT viewId = obs.first; const sfmData::Observation& obs2d = obs.second; - observationsPerView[viewId][landmark.descType].emplace_back(obs2d.id_feat, trackId); + observationsPerView[viewId][landmark.descType].emplace_back(obs2d.getFeatureId(), trackId); } } for (auto& featuresPerTypeInImage : observationsPerView) diff --git a/src/aliceVision/mvsUtils/MultiViewParams.cpp b/src/aliceVision/mvsUtils/MultiViewParams.cpp index 3cb73719a7..8650a81300 100644 --- a/src/aliceVision/mvsUtils/MultiViewParams.cpp +++ b/src/aliceVision/mvsUtils/MultiViewParams.cpp @@ -546,7 +546,7 @@ StaticVector MultiViewParams::findNearestCamsFromLandmarks(int rc, int nbNe const camera::IntrinsicBase* otherIntrinsicPtr = _sfmData.getIntrinsicPtr(otherView.getIntrinsicId()); const double angle = - camera::angleBetweenRays(pose, intrinsicPtr, otherPose, otherIntrinsicPtr, viewObsIt->second.x, observationPair.second.x); + camera::angleBetweenRays(pose, intrinsicPtr, otherPose, otherIntrinsicPtr, viewObsIt->second.getCoordinates(), observationPair.second.getCoordinates()); if (angle < _minViewAngle || angle > _maxViewAngle) continue; @@ -613,7 +613,7 @@ std::vector MultiViewParams::findTileNearestCams(int rc, int nbNearestCams, continue; // landmark R camera observation is in the image full-size ROI - if (!fullsizeRoi.contains(viewObsIt->second.x.x(), viewObsIt->second.x.y())) + if (!fullsizeRoi.contains(viewObsIt->second.getX(), viewObsIt->second.getY())) continue; for (const auto& observationPair : observations) @@ -635,7 +635,7 @@ std::vector MultiViewParams::findTileNearestCams(int rc, int nbNearestCams, const camera::IntrinsicBase* otherIntrinsicPtr = sfmData.getIntrinsicPtr(otherView.getIntrinsicId()); const double angle = - camera::angleBetweenRays(pose, intrinsicPtr, otherPose, otherIntrinsicPtr, viewObsIt->second.x, observationPair.second.x); + camera::angleBetweenRays(pose, intrinsicPtr, otherPose, otherIntrinsicPtr, viewObsIt->second.getCoordinates(), observationPair.second.getCoordinates()); tcScore[tc] += plateauFunction(1, 10, 50, 150, angle); } diff --git a/src/aliceVision/photometricStereo/normalIntegration.cpp b/src/aliceVision/photometricStereo/normalIntegration.cpp index f521d577f4..d065f14926 100644 --- a/src/aliceVision/photometricStereo/normalIntegration.cpp +++ b/src/aliceVision/photometricStereo/normalIntegration.cpp @@ -545,8 +545,8 @@ void adjustScale(const sfmData::SfMData& sfmData, image::Image& initDepth sfmData::Observation observationInCurrentPicture = currentLandmark.observations.at(viewID); - int rowInd = observationInCurrentPicture.x(1); - int colInd = observationInCurrentPicture.x(0); + int rowInd = observationInCurrentPicture.getY(); + int colInd = observationInCurrentPicture.getX(); estimatedDepths(i) = initDepth(rowInd, colInd); } @@ -578,8 +578,8 @@ void getZ0FromLandmarks(const sfmData::SfMData& sfmData, const sfmData::Landmark& currentLandmark = landmarks.at(currentLandmarkIndex); sfmData::Observation observationInCurrentPicture = currentLandmark.observations.at(viewID); - int rowInd = observationInCurrentPicture.x(1); - int colInd = observationInCurrentPicture.x(0); + int rowInd = observationInCurrentPicture.getY(); + int colInd = observationInCurrentPicture.getX(); if (mask(rowInd, colInd) > 0.7) { diff --git a/src/aliceVision/sfm/ResidualErrorFunctor.hpp b/src/aliceVision/sfm/ResidualErrorFunctor.hpp index 0dc6f13a95..54748c42e4 100644 --- a/src/aliceVision/sfm/ResidualErrorFunctor.hpp +++ b/src/aliceVision/sfm/ResidualErrorFunctor.hpp @@ -60,9 +60,9 @@ struct ResidualErrorFunctor_Pinhole // Compute and return the error is the difference between the predicted // and observed position - const T scale(_obs.scale > 0.0 ? _obs.scale : 1.0); - out_residuals[0] = (projected_x - T(_obs.x[0])) / scale; - out_residuals[1] = (projected_y - T(_obs.x[1])) / scale; + const T scale(_obs.getScale() > 0.0 ? _obs.getScale() : 1.0); + out_residuals[0] = (projected_x - T(_obs.getX())) / scale; + out_residuals[1] = (projected_y - T(_obs.getY())) / scale; } template @@ -209,9 +209,9 @@ struct ResidualErrorFunctor_PinholeRadialK1 // Compute and return the error is the difference between the predicted // and observed position - const T scale(_obs.scale > 0.0 ? _obs.scale : 1.0); - out_residuals[0] = (projected_x - T(_obs.x[0])) / scale; - out_residuals[1] = (projected_y - T(_obs.x[1])) / scale; + const T scale(_obs.getScale() > 0.0 ? _obs.getScale() : 1.0); + out_residuals[0] = (projected_x - T(_obs.getX())) / scale; + out_residuals[1] = (projected_y - T(_obs.getY())) / scale; } template @@ -361,9 +361,9 @@ struct ResidualErrorFunctor_PinholeRadialK3 // Compute and return the error is the difference between the predicted // and observed position - const T scale(_obs.scale > 0.0 ? _obs.scale : 1.0); - out_residuals[0] = (projected_x - T(_obs.x[0])) / scale; - out_residuals[1] = (projected_y - T(_obs.x[1])) / scale; + const T scale(_obs.getScale() > 0.0 ? _obs.getScale() : 1.0); + out_residuals[0] = (projected_x - T(_obs.getX())) / scale; + out_residuals[1] = (projected_y - T(_obs.getY())) / scale; } template @@ -513,9 +513,9 @@ struct ResidualErrorFunctor_PinholeBrownT2 // Compute and return the error is the difference between the predicted // and observed position - const T scale(_obs.scale > 0.0 ? _obs.scale : 1.0); - out_residuals[0] = (projected_x - T(_obs.x[0])) / scale; - out_residuals[1] = (projected_y - T(_obs.x[1])) / scale; + const T scale(_obs.getScale() > 0.0 ? _obs.getScale() : 1.0); + out_residuals[0] = (projected_x - T(_obs.getX())) / scale; + out_residuals[1] = (projected_y - T(_obs.getY())) / scale; } template @@ -672,9 +672,9 @@ struct ResidualErrorFunctor_PinholeFisheye // Compute and return the error is the difference between the predicted // and observed position - const T scale(_obs.scale > 0.0 ? _obs.scale : 1.0); - out_residuals[0] = (projected_x - T(_obs.x[0])) / scale; - out_residuals[1] = (projected_y - T(_obs.x[1])) / scale; + const T scale(_obs.getScale() > 0.0 ? _obs.getScale() : 1.0); + out_residuals[0] = (projected_x - T(_obs.getX())) / scale; + out_residuals[1] = (projected_y - T(_obs.getY())) / scale; } template @@ -819,9 +819,9 @@ struct ResidualErrorFunctor_PinholeFisheye1 // Compute and return the error is the difference between the predicted // and observed position - const T scale(_obs.scale > 0.0 ? _obs.scale : 1.0); - out_residuals[0] = (projected_x - T(_obs.x[0])) / scale; - out_residuals[1] = (projected_y - T(_obs.x[1])) / scale; + const T scale(_obs.getScale() > 0.0 ? _obs.getScale() : 1.0); + out_residuals[0] = (projected_x - T(_obs.getX())) / scale; + out_residuals[1] = (projected_y - T(_obs.getY())) / scale; } template @@ -993,9 +993,9 @@ struct ResidualErrorFunctor_Pinhole3DEClassicLD // Compute and return the error is the difference between the predicted // and observed position - const T scale(_obs.scale > 0.0 ? _obs.scale : 1.0); - out_residuals[0] = (projected_x - T(_obs.x[0])) / scale; - out_residuals[1] = (projected_y - T(_obs.x[1])) / scale; + const T scale(_obs.getScale() > 0.0 ? _obs.getScale() : 1.0); + out_residuals[0] = (projected_x - T(_obs.getX())) / scale; + out_residuals[1] = (projected_y - T(_obs.getY())) / scale; } template @@ -1157,9 +1157,9 @@ struct ResidualErrorFunctor_Pinhole3DERadial4 // Compute and return the error is the difference between the predicted // and observed position - const T scale(_obs.scale > 0.0 ? _obs.scale : 1.0); - out_residuals[0] = (projected_x - T(_obs.x[0])) / scale; - out_residuals[1] = (projected_y - T(_obs.x[1])) / scale; + const T scale(_obs.getScale() > 0.0 ? _obs.getScale() : 1.0); + out_residuals[0] = (projected_x - T(_obs.getX())) / scale; + out_residuals[1] = (projected_y - T(_obs.getY())) / scale; } template @@ -1354,9 +1354,9 @@ struct ResidualErrorFunctor_Pinhole3DEAnamorphic4 // Compute and return the error is the difference between the predicted // and observed position - const T scale(_obs.scale > 0.0 ? _obs.scale : 1.0); - out_residuals[0] = (projected_x - T(_obs.x[0])) / scale; - out_residuals[1] = (projected_y - T(_obs.x[1])) / scale; + const T scale(_obs.getScale() > 0.0 ? _obs.getScale() : 1.0); + out_residuals[0] = (projected_x - T(_obs.getX())) / scale; + out_residuals[1] = (projected_y - T(_obs.getY())) / scale; } template diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index 91db63081e..06c4d06d41 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -203,7 +203,7 @@ ceres::CostFunction* createCostFunctionFromIntrinsics(const IntrinsicBase* intri auto undistortion = intrinsicDistortionPtr->getUndistortion(); if (undistortion) { - obsUndistorted.x = undistortion->undistort(observation.x); + obsUndistorted.setCoordinates(undistortion->undistort(observation.getCoordinates())); if (intrinsicDistortionPtr->getDistortion() != nullptr) { @@ -263,7 +263,7 @@ ceres::CostFunction* createRigCostFunctionFromIntrinsics(const IntrinsicBase* in auto undistortion = intrinsicDistortionPtr->getUndistortion(); if (undistortion) { - obsUndistorted.x = undistortion->undistort(observation.x); + obsUndistorted.setCoordinates(undistortion->undistort(observation.getCoordinates())); } } @@ -902,7 +902,7 @@ void BundleAdjustmentCeres::addConstraints2DToProblem(const sfmData::SfMData& sf assert(intrinsicBlockPtr_1 == intrinsicBlockPtr_2); ceres::CostFunction* costFunction = createConstraintsCostFunctionFromIntrinsics( - sfmData.getIntrinsicPtr(view_1.getIntrinsicId()), constraint.ObservationFirst.x, constraint.ObservationSecond.x); + sfmData.getIntrinsicPtr(view_1.getIntrinsicId()), constraint.ObservationFirst.getCoordinates(), constraint.ObservationSecond.getCoordinates()); problem.AddResidualBlock(costFunction, lossFunction, intrinsicBlockPtr_1, poseBlockPtr_1, poseBlockPtr_2); } } diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp index 74a974a212..e92be38a8e 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp @@ -594,19 +594,19 @@ void BundleAdjustmentSymbolicCeres::addConstraints2DToProblem(const sfmData::SfM if (equidistant != nullptr) { ceres::CostFunction* costFunction = - new CostPanoramaEquidistant(constraint.ObservationFirst.x, constraint.ObservationSecond.x, equidistant); + new CostPanoramaEquidistant(constraint.ObservationFirst.getCoordinates(), constraint.ObservationSecond.getCoordinates(), equidistant); problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2, intrinsicBlockPtr_1); /* Symmetry */ - costFunction = new CostPanoramaEquidistant(constraint.ObservationSecond.x, constraint.ObservationFirst.x, equidistant); + costFunction = new CostPanoramaEquidistant(constraint.ObservationSecond.getCoordinates(), constraint.ObservationFirst.getCoordinates(), equidistant); problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_2, poseBlockPtr_1, intrinsicBlockPtr_1); } else if (pinhole != nullptr) { - ceres::CostFunction* costFunction = new CostPanoramaPinHole(constraint.ObservationFirst.x, constraint.ObservationSecond.x, pinhole); + ceres::CostFunction* costFunction = new CostPanoramaPinHole(constraint.ObservationFirst.getCoordinates(), constraint.ObservationSecond.getCoordinates(), pinhole); problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_1, poseBlockPtr_2, intrinsicBlockPtr_1); /* Symmetry */ - costFunction = new CostPanoramaPinHole(constraint.ObservationSecond.x, constraint.ObservationFirst.x, pinhole); + costFunction = new CostPanoramaPinHole(constraint.ObservationSecond.getCoordinates(), constraint.ObservationFirst.getCoordinates(), pinhole); problem.AddResidualBlock(costFunction, lossFunction, poseBlockPtr_2, poseBlockPtr_1, intrinsicBlockPtr_1); } else diff --git a/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp b/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp index dc0c553883..ba908a9230 100644 --- a/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp +++ b/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp @@ -254,7 +254,7 @@ double RMSE(const SfMData& sfm_data) const View* view = sfm_data.getViews().find(itObs->first)->second.get(); const Pose3 pose = sfm_data.getPose(*view).getTransform(); const std::shared_ptr intrinsic = sfm_data.getIntrinsics().find(view->getIntrinsicId())->second; - const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.x); + const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.getCoordinates()); vec.push_back(residual(0)); vec.push_back(residual(1)); } diff --git a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp index d658f00dc7..5048fd3dd3 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp @@ -53,10 +53,10 @@ class CostProjection : public ceres::CostFunction const Vec4 pth = pt.homogeneous(); const Vec2 pt_est = _intrinsics->project(T_pose3, pth, true); - const double scale = (_measured.scale > 1e-12) ? _measured.scale : 1.0; + const double scale = (_measured.getScale() > 1e-12) ? _measured.getScale() : 1.0; - residuals[0] = (pt_est(0) - _measured.x(0)) / scale; - residuals[1] = (pt_est(1) - _measured.x(1)) / scale; + residuals[0] = (pt_est(0) - _measured.getX()) / scale; + residuals[1] = (pt_est(1) - _measured.getY()) / scale; if (jacobians == nullptr) { diff --git a/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp b/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp index 3cd1a85164..58d67b8df9 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/projectionSimple.hpp @@ -39,10 +39,10 @@ class CostProjectionSimple : public ceres::CostFunction const Vec4 cpt = T * pth; Vec2 pt_est = _intrinsics->project(T, pth, false); - const double scale = (_measured.scale > 1e-12) ? _measured.scale : 1.0; + const double scale = (_measured.getScale() > 1e-12) ? _measured.getScale() : 1.0; - residuals[0] = (pt_est(0) - _measured.x(0)) / scale; - residuals[1] = (pt_est(1) - _measured.x(1)) / scale; + residuals[0] = (pt_est(0) - _measured.getX()) / scale; + residuals[1] = (pt_est(1) - _measured.getY()) / scale; if (jacobians == nullptr) { diff --git a/src/aliceVision/sfm/generateReport.cpp b/src/aliceVision/sfm/generateReport.cpp index dbd74b9e91..6e695390ca 100644 --- a/src/aliceVision/sfm/generateReport.cpp +++ b/src/aliceVision/sfm/generateReport.cpp @@ -33,7 +33,7 @@ bool generateSfMReport(const sfmData::SfMData& sfmData, const std::string& htmlF const geometry::Pose3 pose = sfmData.getPose(*view).getTransform(); const camera::IntrinsicBase* intrinsic = sfmData.getIntrinsics().at(view->getIntrinsicId()).get(); // Use absolute values - const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.x).array().abs(); + const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.getCoordinates()).array().abs(); residuals_per_view[itObs->first].push_back(residual(0)); residuals_per_view[itObs->first].push_back(residual(1)); ++residualCount; diff --git a/src/aliceVision/sfm/pipeline/ReconstructionEngine.cpp b/src/aliceVision/sfm/pipeline/ReconstructionEngine.cpp index 382b2c1cf0..46b867d042 100644 --- a/src/aliceVision/sfm/pipeline/ReconstructionEngine.cpp +++ b/src/aliceVision/sfm/pipeline/ReconstructionEngine.cpp @@ -61,7 +61,7 @@ void retrieveMarkersId(sfmData::SfMData& sfmData) const feature::APRILTAG_Regions* apriltagRegions = dynamic_cast(®ions); if (cctagRegions) { - const auto& d = cctagRegions->Descriptors()[obs->second.id_feat]; + const auto& d = cctagRegions->Descriptors()[obs->second.getFeatureId()]; for (int i = 0; i < d.size(); ++i) { if (d[i] == 255) @@ -74,7 +74,7 @@ void retrieveMarkersId(sfmData::SfMData& sfmData) } else if (apriltagRegions) { - const auto& d = apriltagRegions->Descriptors()[obs->second.id_feat]; + const auto& d = apriltagRegions->Descriptors()[obs->second.getFeatureId()]; for (int i = 0; i < d.size(); ++i) { if (d[i] == 255) diff --git a/src/aliceVision/sfm/pipeline/RigSequence.cpp b/src/aliceVision/sfm/pipeline/RigSequence.cpp index bdd3f13299..4b6cf77758 100644 --- a/src/aliceVision/sfm/pipeline/RigSequence.cpp +++ b/src/aliceVision/sfm/pipeline/RigSequence.cpp @@ -68,7 +68,7 @@ double computeCameraScore(const SfMData& sfmData, const track::TracksPerView& tr if (itObs != landmark.observations.end()) { - const Vec2 residual = intrinsic->residual(pose, landmark.X.homogeneous(), itObs->second.x); + const Vec2 residual = intrinsic->residual(pose, landmark.X.homogeneous(), itObs->second.getCoordinates()); score += std::min(1.0 / residual.norm(), 4.0); } } diff --git a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp index bfbb85ab95..489a99cf01 100644 --- a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp +++ b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp @@ -922,12 +922,12 @@ bool ReconstructionEngine_panorama::buildLandmarks() const sfmData::View& v1 = _sfmData.getView(c.ViewFirst); const std::shared_ptr cam1 = _sfmData.getIntrinsicsharedPtr(v1.getIntrinsicId()); const sfmData::CameraPose pose1 = _sfmData.getPose(v1); - const Vec3 wpt1 = cam1->backproject(c.ObservationFirst.x, true, pose1.getTransform(), 1.0); + const Vec3 wpt1 = cam1->backproject(c.ObservationFirst.getCoordinates(), true, pose1.getTransform(), 1.0); const sfmData::View& v2 = _sfmData.getView(c.ViewSecond); const std::shared_ptr cam2 = _sfmData.getIntrinsicsharedPtr(v2.getIntrinsicId()); const sfmData::CameraPose pose2 = _sfmData.getPose(v2); - const Vec3 wpt2 = cam2->backproject(c.ObservationSecond.x, true, pose2.getTransform(), 1.0); + const Vec3 wpt2 = cam2->backproject(c.ObservationSecond.getCoordinates(), true, pose2.getTransform(), 1.0); // Store landmark Landmark l; diff --git a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp index f7578e6a3c..0757115755 100644 --- a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp +++ b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp @@ -425,7 +425,7 @@ void ReconstructionEngine_sequentialSfM::remapLandmarkIdsToTrackIds() { const IndexT landmarkId = landmarkPair.first; const IndexT firstViewId = landmarkPair.second.observations.begin()->first; - const IndexT firstFeatureId = landmarkPair.second.observations.begin()->second.id_feat; + const IndexT firstFeatureId = landmarkPair.second.observations.begin()->second.getFeatureId(); const feature::EImageDescriberType descType = landmarkPair.second.descType; obsToLandmark.emplace(ObsKey(firstViewId, firstFeatureId, descType), landmarkId); diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp index 35a37e642c..a24cd75a24 100644 --- a/src/aliceVision/sfm/sfmFilters.cpp +++ b/src/aliceVision/sfm/sfmFilters.cpp @@ -35,12 +35,12 @@ IndexT RemoveOutliers_PixelResidualError(sfmData::SfMData& sfmData, const geometry::Pose3 pose = sfmData.getPose(*view).getTransform(); const camera::IntrinsicBase* intrinsic = sfmData.getIntrinsics().at(view->getIntrinsicId()).get(); - Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.x); - if (featureConstraint == EFeatureConstraint::SCALE && itObs->second.scale > 0.0) + Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.getCoordinates()); + if (featureConstraint == EFeatureConstraint::SCALE && itObs->second.getScale() > 0.0) { // Apply the scale of the feature to get a residual value // relative to the feature precision. - residual /= itObs->second.scale; + residual /= itObs->second.getScale(); } if ((pose.depth(iterTracks->second.X) < 0) || (residual.norm() > dThresholdPixel)) @@ -96,7 +96,7 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc const geometry::Pose3 pose = sfmData.getPose(*view).getTransform(); const camera::IntrinsicBase* intrinsic = sfmData.getIntrinsics().at(view->getIntrinsicId()).get(); - viewDirections.col(i) = applyIntrinsicExtrinsic(pose, intrinsic, itObs->second.x); + viewDirections.col(i) = applyIntrinsicExtrinsic(pose, intrinsic, itObs->second.getCoordinates()); double dCosAngle = viewDirections.col(i).transpose() * viewDirections.col(greedyI); if (dCosAngle < dMaxAcceptedCosAngle) diff --git a/src/aliceVision/sfm/sfmStatistics.cpp b/src/aliceVision/sfm/sfmStatistics.cpp index 8e0de96781..e011d58b3d 100644 --- a/src/aliceVision/sfm/sfmStatistics.cpp +++ b/src/aliceVision/sfm/sfmStatistics.cpp @@ -48,7 +48,7 @@ void computeResidualsHistogram(const sfmData::SfMData& sfmData, const sfmData::View& view = sfmData.getView(obs.first); const aliceVision::geometry::Pose3 pose = sfmData.getPose(view).getTransform(); const std::shared_ptr intrinsic = sfmData.getIntrinsics().find(view.getIntrinsicId())->second; - const Vec2 residual = intrinsic->residual(pose, track.second.X.homogeneous(), obs.second.x); + const Vec2 residual = intrinsic->residual(pose, track.second.X.homogeneous(), obs.second.getCoordinates()); vec_residuals.push_back(residual.norm()); // ALICEVISION_LOG_INFO("[AliceVision] sfmtstatistics::computeResidualsHistogram track: " << track.first << ", residual: " << // residual.norm()); @@ -292,7 +292,7 @@ void computeScaleHistogram(const sfmData::SfMData& sfmData, if (specificViews.count(obs.first) == 0) continue; } - vec_scaleObservations.push_back(obs.second.scale); + vec_scaleObservations.push_back(obs.second.getScale()); } } @@ -340,7 +340,7 @@ void computeResidualsPerView(const sfmData::SfMData& sfmData, const sfmData::View& view = sfmData.getView(obs.first); const aliceVision::geometry::Pose3 pose = sfmData.getPose(view).getTransform(); const std::shared_ptr intrinsic = sfmData.getIntrinsics().find(view.getIntrinsicId())->second; - const Vec2 residual = intrinsic->residual(pose, landmark.second.X.homogeneous(), obs.second.x); + const Vec2 residual = intrinsic->residual(pose, landmark.second.X.homogeneous(), obs.second.getCoordinates()); residualsPerView[obs.first].push_back(residual.norm()); } } diff --git a/src/aliceVision/sfm/sfmTriangulation.cpp b/src/aliceVision/sfm/sfmTriangulation.cpp index 17fca92341..aa7437f2c0 100644 --- a/src/aliceVision/sfm/sfmTriangulation.cpp +++ b/src/aliceVision/sfm/sfmTriangulation.cpp @@ -61,7 +61,7 @@ void StructureComputation_blind::triangulate(sfmData::SfMData& sfmData, std::mt1 } const Pose3 pose = sfmData.getPose(*view).getTransform(); - trianObj.add(pinHoleCam->getProjectiveEquivalent(pose), cam->get_ud_pixel(itObs.second.x)); + trianObj.add(pinHoleCam->getProjectiveEquivalent(pose), cam->get_ud_pixel(itObs.second.getCoordinates())); } } if (trianObj.size() < 2) @@ -211,7 +211,7 @@ bool StructureComputation_robust::robust_triangulation(const sfmData::SfMData& s const sfmData::View* view = sfmData.getViews().at(itObs.first).get(); const IntrinsicBase* intrinsic = sfmData.getIntrinsics().at(view->getIntrinsicId()).get(); const Pose3 pose = sfmData.getPose(*view).getTransform(); - const Vec2 residual = intrinsic->residual(pose, current_model.homogeneous(), itObs.second.x); + const Vec2 residual = intrinsic->residual(pose, current_model.homogeneous(), itObs.second.getCoordinates()); const double residual_d = residual.norm(); if (residual_d < dThresholdPixel) @@ -257,7 +257,7 @@ Vec3 StructureComputation_robust::track_sample_triangulation(const sfmData::SfMD } const Pose3 pose = sfmData.getPose(*view).getTransform(); - trianObj.add(camPinHole->getProjectiveEquivalent(pose), cam->get_ud_pixel(itObs->second.x)); + trianObj.add(camPinHole->getProjectiveEquivalent(pose), cam->get_ud_pixel(itObs->second.getCoordinates())); } return trianObj.compute(); } diff --git a/src/aliceVision/sfm/utils/statistics.cpp b/src/aliceVision/sfm/utils/statistics.cpp index d5fd210efc..28f8b0a7ae 100644 --- a/src/aliceVision/sfm/utils/statistics.cpp +++ b/src/aliceVision/sfm/utils/statistics.cpp @@ -23,7 +23,7 @@ double RMSE(const sfmData::SfMData& sfmData) const sfmData::View* view = sfmData.getViews().find(itObs->first)->second.get(); const geometry::Pose3 pose = sfmData.getPose(*view).getTransform(); const std::shared_ptr intrinsic = sfmData.getIntrinsics().at(view->getIntrinsicId()); - const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.x); + const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.getCoordinates()); vec.push_back(residual(0)); vec.push_back(residual(1)); } diff --git a/src/aliceVision/sfm/utils/syntheticScene.cpp b/src/aliceVision/sfm/utils/syntheticScene.cpp index ca959d6c17..e12f00f92e 100644 --- a/src/aliceVision/sfm/utils/syntheticScene.cpp +++ b/src/aliceVision/sfm/utils/syntheticScene.cpp @@ -36,7 +36,7 @@ void generateSyntheticMatches(matching::PairwiseMatches& out_pairwiseMatches, co const sfmData::Observation& obsJ = obsItJ->second; - out_pairwiseMatches[Pair(obsItI->first, obsItJ->first)][descType].emplace_back(obsItI->second.id_feat, obsItJ->second.id_feat); + out_pairwiseMatches[Pair(obsItI->first, obsItJ->first)][descType].emplace_back(obsItI->second.getFeatureId(), obsItJ->second.getFeatureId()); } } } diff --git a/src/aliceVision/sfm/utils/syntheticScene.hpp b/src/aliceVision/sfm/utils/syntheticScene.hpp index 571eab46c0..7d6430809d 100644 --- a/src/aliceVision/sfm/utils/syntheticScene.hpp +++ b/src/aliceVision/sfm/utils/syntheticScene.hpp @@ -46,7 +46,7 @@ void generateSyntheticFeatures(feature::FeaturesPerView& out_featuresPerView, { const IndexT viewId = obsIt.first; const sfmData::Observation& obs = obsIt.second; - nbFeatPerView[viewId] = std::max(nbFeatPerView[viewId], std::size_t(obs.id_feat + 1)); + nbFeatPerView[viewId] = std::max(nbFeatPerView[viewId], std::size_t(obs.getFeatureId() + 1)); } } for (auto& it : nbFeatPerView) @@ -70,8 +70,8 @@ void generateSyntheticFeatures(feature::FeaturesPerView& out_featuresPerView, const IndexT viewId = obsIt.first; const sfmData::Observation& obs = obsIt.second; - out_featuresPerView.getFeaturesPerDesc(viewId)[descType][obs.id_feat] = - feature::PointFeature(obs.x(0) + noise(generator), obs.x(1) + noise(generator), scale, orientation); + out_featuresPerView.getFeaturesPerDesc(viewId)[descType][obs.getFeatureId()] = + feature::PointFeature(obs.getX() + noise(generator), obs.getY() + noise(generator), scale, orientation); } } } diff --git a/src/aliceVision/sfmData/CMakeLists.txt b/src/aliceVision/sfmData/CMakeLists.txt index fb5cad4658..71a51dd5a7 100644 --- a/src/aliceVision/sfmData/CMakeLists.txt +++ b/src/aliceVision/sfmData/CMakeLists.txt @@ -10,6 +10,7 @@ set(sfmData_files_headers exif.hpp imageInfo.hpp exposureSetting.hpp + Observation.hpp ) # Sources @@ -20,6 +21,7 @@ set(sfmData_files_sources colorize.cpp exif.cpp imageInfo.cpp + Observation.cpp ) alicevision_add_library(aliceVision_sfmData diff --git a/src/aliceVision/sfmData/Landmark.hpp b/src/aliceVision/sfmData/Landmark.hpp index 8ea898ed78..119492e8c6 100644 --- a/src/aliceVision/sfmData/Landmark.hpp +++ b/src/aliceVision/sfmData/Landmark.hpp @@ -12,31 +12,11 @@ #include #include #include +#include namespace aliceVision { namespace sfmData { -/** - * @brief 2D observation of a 3D landmark. - */ -struct Observation -{ - Observation() {} - Observation(const Vec2& p, IndexT idFeat, double scale_) - : x(p), - id_feat(idFeat), - scale(scale_) - {} - - Vec2 x; - IndexT id_feat = UndefinedIndexT; - double scale = 0.0; - bool operator==(const Observation& other) const { return AreVecNearEqual(x, other.x, 1e-6) && id_feat == other.id_feat; } -}; - -/// Observations are indexed by their View_id -typedef stl::flat_map Observations; - /** * @brief Landmark is a 3D point with its 2d observations. */ diff --git a/src/aliceVision/sfmData/Observation.cpp b/src/aliceVision/sfmData/Observation.cpp new file mode 100644 index 0000000000..0ce5b99edc --- /dev/null +++ b/src/aliceVision/sfmData/Observation.cpp @@ -0,0 +1,18 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 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 "Observation.hpp" + +namespace aliceVision { +namespace sfmData { + +bool Observation::operator==(const Observation& other) const +{ + return AreVecNearEqual(_coordinates, other._coordinates, 1e-6) && _idFeature == other._idFeature; +} + +} // namespace sfmData +} // namespace aliceVision diff --git a/src/aliceVision/sfmData/Observation.hpp b/src/aliceVision/sfmData/Observation.hpp new file mode 100644 index 0000000000..575ea9577b --- /dev/null +++ b/src/aliceVision/sfmData/Observation.hpp @@ -0,0 +1,98 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 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 +#include + +namespace aliceVision { +namespace sfmData { + +/** + * @brief 2D observation of a 3D landmark. + */ +class Observation +{ +public: + Observation() + { + } + + Observation(const Vec2& p, IndexT idFeat, double scale_) + : _coordinates(p), + _idFeature(idFeat), + _scale(scale_) + { + } + + bool operator==(const Observation& other) const; + + const Vec2 & getCoordinates() const + { + return _coordinates; + } + + Vec2 & getCoordinates() + { + return _coordinates; + } + + double getX() const + { + return _coordinates.x(); + } + + double getY() const + { + return _coordinates.y(); + } + + void setCoordinates(const Vec2 & coordinates) + { + _coordinates = coordinates; + } + + void setCoordinates(double x, double y) + { + _coordinates(0) = x; + _coordinates(1) = y; + } + + IndexT getFeatureId() const + { + return _idFeature; + } + + void setFeatureId(IndexT featureId) + { + _idFeature = featureId; + } + + double getScale() const + { + return _scale; + } + + void setScale(double scale) + { + _scale = scale; + } + +private: + Vec2 _coordinates; + IndexT _idFeature = UndefinedIndexT; + double _scale = 0.0; +}; + +/// Observations are indexed by their View_id +typedef stl::flat_map Observations; + +} // namespace sfmData +} // namespace aliceVision diff --git a/src/aliceVision/sfmData/colorize.cpp b/src/aliceVision/sfmData/colorize.cpp index abcc3a0b45..80d5c5c8ae 100644 --- a/src/aliceVision/sfmData/colorize.cpp +++ b/src/aliceVision/sfmData/colorize.cpp @@ -110,7 +110,7 @@ void colorizeTracks(SfMData& sfmData) for (Landmark& landmark : viewCardinal.landmarks) { // color the point - Vec2 pt = landmark.observations.at(view.getViewId()).x; + Vec2 pt = landmark.observations.at(view.getViewId()).getCoordinates(); // clamp the pixel position if the feature/marker center is outside the image. pt.x() = clamp(pt.x(), 0.0, static_cast(image.Width() - 1)); pt.y() = clamp(pt.y(), 0.0, static_cast(image.Height() - 1)); diff --git a/src/aliceVision/sfmDataIO/AlembicExporter.cpp b/src/aliceVision/sfmDataIO/AlembicExporter.cpp index e719730bc9..eb62f750e7 100644 --- a/src/aliceVision/sfmDataIO/AlembicExporter.cpp +++ b/src/aliceVision/sfmDataIO/AlembicExporter.cpp @@ -549,13 +549,13 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks, if (withFeatures) { // featureId - visibilityFeatId.emplace_back(obs.id_feat); + visibilityFeatId.emplace_back(obs.getFeatureId()); // feature 2D position (x, y)) - featPos2d.emplace_back(obs.x[0]); - featPos2d.emplace_back(obs.x[1]); + featPos2d.emplace_back(obs.getX()); + featPos2d.emplace_back(obs.getY()); - featScale.emplace_back(obs.scale); + featScale.emplace_back(obs.getScale()); } } } diff --git a/src/aliceVision/sfmDataIO/AlembicImporter.cpp b/src/aliceVision/sfmDataIO/AlembicImporter.cpp index 47602b4949..8ae012f77b 100644 --- a/src/aliceVision/sfmDataIO/AlembicImporter.cpp +++ b/src/aliceVision/sfmDataIO/AlembicImporter.cpp @@ -263,12 +263,11 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData:: const int viewID = sampleVisibilityIds[obsGlobal_i]; const int featID = sampleVisibilityIds[obsGlobal_i + 1]; sfmData::Observation& observations = landmark.observations[viewID]; - observations.id_feat = featID; + observations.setFeatureId(featID); const float posX = (*sampleFeatPos2d)[obsGlobal_i]; const float posY = (*sampleFeatPos2d)[obsGlobal_i + 1]; - observations.x[0] = posX; - observations.x[1] = posY; + observations.setCoordinates(posX, posY); } } } @@ -350,17 +349,16 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData:: { const std::size_t featId = sampleVisibilityFeatId[obsGlobalIndex]; sfmData::Observation& observation = landmark.observations[viewId]; - observation.id_feat = featId; + observation.setFeatureId(featId); const float posX = (*sampleVisibilityFeatPos)[2 * obsGlobalIndex]; const float posY = (*sampleVisibilityFeatPos)[2 * obsGlobalIndex + 1]; - observation.x[0] = posX; - observation.x[1] = posY; + observation.setCoordinates(posX, posY); // for compatibility with previous version without scale if (sampleVisibilityFeatScale) { - observation.scale = (*sampleVisibilityFeatScale)[obsGlobalIndex]; + observation.setScale((*sampleVisibilityFeatScale)[obsGlobalIndex]); } } else diff --git a/src/aliceVision/sfmDataIO/bafIO.cpp b/src/aliceVision/sfmDataIO/bafIO.cpp index 9abf6c8332..5fdf77db91 100644 --- a/src/aliceVision/sfmDataIO/bafIO.cpp +++ b/src/aliceVision/sfmDataIO/bafIO.cpp @@ -73,7 +73,7 @@ bool saveBAF(const sfmData::SfMData& sfmData, const std::string& filename, ESfMD { const IndexT id_view = iterOb->first; const sfmData::View* v = sfmData.getViews().at(id_view).get(); - stream << v->getIntrinsicId() << ' ' << v->getPoseId() << ' ' << iterOb->second.x(0) << ' ' << iterOb->second.x(1) << ' '; + stream << v->getIntrinsicId() << ' ' << v->getPoseId() << ' ' << iterOb->second.getX() << ' ' << iterOb->second.getY() << ' '; } stream << '\n'; } diff --git a/src/aliceVision/sfmDataIO/colmap.cpp b/src/aliceVision/sfmDataIO/colmap.cpp index 0e509ffdd7..d283fcd5d5 100644 --- a/src/aliceVision/sfmDataIO/colmap.cpp +++ b/src/aliceVision/sfmDataIO/colmap.cpp @@ -257,7 +257,7 @@ PerViewVisibility computePerViewVisibility(const sfmData::SfMData& sfmData, cons if (viewSelections.find(viewID) != viewSelections.end()) { // for the current viewID add the feature point and its associate 3D point's ID - perCameraVisibility[viewID][landID] = obs.x; + perCameraVisibility[viewID][landID] = obs.getCoordinates(); } } } @@ -365,7 +365,7 @@ void generateColmapPoints3DTxtFile(const sfmData::SfMData& sfmData, const Compat for (const auto& itObs : iter.second.observations) { const IndexT viewId = itObs.first; - const IndexT featId = itObs.second.id_feat; + const IndexT featId = itObs.second.getFeatureId(); if (viewSelections.find(viewId) != viewSelections.end()) { diff --git a/src/aliceVision/sfmDataIO/jsonIO.cpp b/src/aliceVision/sfmDataIO/jsonIO.cpp index 0ed9292aa5..d1a8446d71 100644 --- a/src/aliceVision/sfmDataIO/jsonIO.cpp +++ b/src/aliceVision/sfmDataIO/jsonIO.cpp @@ -463,9 +463,9 @@ void saveLandmark(const std::string& name, // features if (saveFeatures) { - obsTree.put("featureId", observation.id_feat); - saveMatrix("x", observation.x, obsTree); - obsTree.put("scale", observation.scale); + obsTree.put("featureId", observation.getFeatureId()); + saveMatrix("x", observation.getCoordinates(), obsTree); + obsTree.put("scale", observation.getScale()); } observationsTree.push_back(std::make_pair("", obsTree)); @@ -496,9 +496,9 @@ void loadLandmark(IndexT& landmarkId, sfmData::Landmark& landmark, bpt::ptree& l if (loadFeatures) { - observation.id_feat = obsTree.get("featureId"); - loadMatrix("x", observation.x, obsTree); - observation.scale = obsTree.get("scale", 0.0); + observation.setFeatureId(obsTree.get("featureId")); + loadMatrix("x", observation.getCoordinates(), obsTree); + observation.setScale(obsTree.get("scale", 0.0)); } landmark.observations.emplace(obsTree.get("observationId"), observation); diff --git a/src/software/export/main_exportMVE2.cpp b/src/software/export/main_exportMVE2.cpp index d96d260424..bd59af251c 100644 --- a/src/software/export/main_exportMVE2.cpp +++ b/src/software/export/main_exportMVE2.cpp @@ -235,7 +235,7 @@ bool exportToMVE2Format( { const IndexT viewId = itObs->first; const IndexT viewIndex = viewIdToviewIndex[viewId]; - const IndexT featId = itObs->second.id_feat; + const IndexT featId = itObs->second.getFeatureId(); out << " " << viewIndex << " " << featId << " 0"; } out << "\n"; diff --git a/src/software/export/main_exportMatlab.cpp b/src/software/export/main_exportMatlab.cpp index 7e99780e55..bb41b0488d 100644 --- a/src/software/export/main_exportMatlab.cpp +++ b/src/software/export/main_exportMatlab.cpp @@ -56,7 +56,7 @@ bool exportToMatlab( for(const auto& obs: landmark.observations) { const IndexT obsView = obs.first; // The ID of the view that provides this 2D observation. - observationsPerView[obsView].push_back(Observation(obs.second.x, landmarkId, unknownScale)); + observationsPerView[obsView].push_back(Observation(obs.second.getCoordinates(), landmarkId, unknownScale)); } } landmarksFile.close(); @@ -73,7 +73,7 @@ bool exportToMatlab( viewFeatFile << "# landmarkId x y\n"; for(const Observation& obs: viewObservations) { - viewFeatFile << obs.id_feat << " " << obs.x[0] << " " << obs.x[1] << "\n"; + viewFeatFile << obs.getFeatureId() << " " << obs.getX() << " " << obs.getY() << "\n"; } viewFeatFile.close(); } diff --git a/src/software/export/main_exportPMVS.cpp b/src/software/export/main_exportPMVS.cpp index 2451fc1a99..ad5b04ac56 100644 --- a/src/software/export/main_exportPMVS.cpp +++ b/src/software/export/main_exportPMVS.cpp @@ -315,7 +315,7 @@ bool exportToBundlerFormat( { const Observation & ob = iterObs->second; // ViewId, FeatId, x, y - os << map_viewIdToContiguous[iterObs->first] << " " << ob.id_feat << " " << ob.x(0) << " " << ob.x(1) << " "; + os << map_viewIdToContiguous[iterObs->first] << " " << ob.getFeatureId() << " " << ob.getX() << " " << ob.getY() << " "; } os << os.widen('\n'); } diff --git a/src/software/pipeline/main_sfmBootstraping.cpp b/src/software/pipeline/main_sfmBootstraping.cpp index 7611faa90e..92555a52cc 100644 --- a/src/software/pipeline/main_sfmBootstraping.cpp +++ b/src/software/pipeline/main_sfmBootstraping.cpp @@ -268,14 +268,14 @@ bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pai landmark.X = X; sfmData::Observation refObs; - refObs.id_feat = refFeatureId; - refObs.scale = refFeatures[refFeatureId].scale(); - refObs.x = refpt; + refObs.setFeatureId(refFeatureId); + refObs.setScale(refFeatures[refFeatureId].scale()); + refObs.setCoordinates(refpt); sfmData::Observation nextObs; - nextObs.id_feat = nextFeatureId; - nextObs.scale = nextFeatures[nextFeatureId].scale(); - nextObs.x = nextpt; + nextObs.setFeatureId(nextFeatureId); + nextObs.setScale(nextFeatures[nextFeatureId].scale()); + nextObs.setCoordinates(nextpt); landmark.observations[pair.reference] = refObs; landmark.observations[pair.next] = nextObs; diff --git a/src/software/utils/main_sfmRegression.cpp b/src/software/utils/main_sfmRegression.cpp index c7c2e03aa7..9943270645 100644 --- a/src/software/utils/main_sfmRegression.cpp +++ b/src/software/utils/main_sfmRegression.cpp @@ -106,11 +106,7 @@ void generateSampleSceneOnePlane(sfmData::SfMData & returnSfmDataGT, sfmData::Sf for (auto & pp : sfmDataGT.getPoses()) { - sfmData::Observation obs; - obs.x = phPinholeGT->project(pp.second.getTransform(), pl.second.X.homogeneous(), true); - obs.scale = 1.0; - obs.id_feat = pl.first; - + sfmData::Observation obs(phPinholeGT->project(pp.second.getTransform(), pl.second.X.homogeneous(), true), pl.first, 1.0); if (pp.second.getTransform()(pl.second.X)(2) < 0.1) { From c8d57ce60eeb422e3c1968989134f06d0566f5fc Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Fri, 1 Dec 2023 16:54:13 +0100 Subject: [PATCH 2/3] Transform landmark to a class --- src/aliceVision/depthMap/SgmDepthList.cpp | 10 ++-- src/aliceVision/depthMap/volumeIO.cpp | 12 ++--- src/aliceVision/fuseCut/DelaunayGraphCut.cpp | 4 +- .../fuseCut/DelaunayGraphCut_test.cpp | 2 +- src/aliceVision/fuseCut/Fuser.cpp | 8 ++-- .../localization/CCTagLocalizer.cpp | 2 +- .../localization/VoctreeLocalizer.cpp | 2 +- src/aliceVision/localization/optimization.cpp | 18 +++---- .../resection/resectionLORansac_test.cpp | 2 +- src/aliceVision/mvsUtils/MultiViewParams.cpp | 4 +- .../photometricStereo/normalIntegration.cpp | 4 +- src/aliceVision/sfm/FrustumFilter.cpp | 2 +- .../sfm/LocalBundleAdjustmentGraph.cpp | 4 +- .../sfm/bundle/BundleAdjustmentCeres.cpp | 2 +- .../bundle/BundleAdjustmentSymbolicCeres.cpp | 2 +- .../sfm/bundle/bundleAdjustment_test.cpp | 16 +++---- src/aliceVision/sfm/generateReport.cpp | 2 +- .../sfm/pipeline/ReconstructionEngine.cpp | 4 +- src/aliceVision/sfm/pipeline/RigSequence.cpp | 4 +- .../global/ReconstructionEngine_globalSfM.cpp | 4 +- .../pipeline/localization/SfMLocalizer.cpp | 2 +- .../ReconstructionEngine_panorama.cpp | 4 +- .../ReconstructionEngine_sequentialSfM.cpp | 24 +++++----- .../StructureEstimationFromKnownPoses.cpp | 2 +- src/aliceVision/sfm/sfmFilters.cpp | 8 ++-- src/aliceVision/sfm/sfmStatistics.cpp | 14 +++--- src/aliceVision/sfm/sfmTriangulation.cpp | 4 +- src/aliceVision/sfm/utils/alignment.cpp | 4 +- src/aliceVision/sfm/utils/alignment_test.cpp | 2 +- src/aliceVision/sfm/utils/statistics.cpp | 2 +- src/aliceVision/sfm/utils/syntheticScene.cpp | 12 ++--- src/aliceVision/sfm/utils/syntheticScene.hpp | 4 +- src/aliceVision/sfmData/Landmark.hpp | 45 ++++++++++++++---- src/aliceVision/sfmData/SfMData.cpp | 2 +- src/aliceVision/sfmData/colorize.cpp | 8 ++-- src/aliceVision/sfmData/uid.cpp | 8 ++-- src/aliceVision/sfmDataIO/AlembicExporter.cpp | 4 +- src/aliceVision/sfmDataIO/AlembicImporter.cpp | 6 +-- src/aliceVision/sfmDataIO/alembicIO_test.cpp | 2 +- src/aliceVision/sfmDataIO/bafIO.cpp | 2 +- src/aliceVision/sfmDataIO/colmap.cpp | 4 +- .../compatibilityData/scene_v1.2.6.abc | Bin 421271 -> 421271 bytes src/aliceVision/sfmDataIO/jsonIO.cpp | 4 +- src/aliceVision/sfmDataIO/sfmDataIO_test.cpp | 2 +- src/aliceVision/sfmMvsUtils/visibility.cpp | 4 +- .../convert/main_convertSfMFormat.cpp | 6 +-- src/software/export/main_exportMVE2.cpp | 2 +- src/software/export/main_exportMatlab.cpp | 2 +- src/software/export/main_exportPMVS.cpp | 4 +- .../pipeline/main_checkerboardCalibration.cpp | 2 +- src/software/pipeline/main_meshing.cpp | 4 +- src/software/pipeline/main_nodalSfM.cpp | 10 ++-- .../pipeline/main_panoramaEstimation.cpp | 2 +- .../pipeline/main_sfmBootstraping.cpp | 4 +- src/software/utils/main_frustumFiltering.cpp | 6 +-- src/software/utils/main_sfmRegression.cpp | 4 +- src/software/utils/main_sfmTransfer.cpp | 8 ++-- 57 files changed, 180 insertions(+), 155 deletions(-) diff --git a/src/aliceVision/depthMap/SgmDepthList.cpp b/src/aliceVision/depthMap/SgmDepthList.cpp index c898aae318..eae867e869 100644 --- a/src/aliceVision/depthMap/SgmDepthList.cpp +++ b/src/aliceVision/depthMap/SgmDepthList.cpp @@ -298,10 +298,10 @@ void SgmDepthList::getMinMaxMidNbDepthFromSfM(float& out_min, float& out_max, fl const Point3d point(landmark.X(0), landmark.X(1), landmark.X(2)); // find rc observation - const auto it = landmark.observations.find(viewId); + const auto it = landmark.getObservations().find(viewId); // no rc observation - if (it == landmark.observations.end()) + if (it == landmark.getObservations().end()) continue; // get rc 2d observation @@ -370,14 +370,14 @@ void SgmDepthList::getRcTcDepthRangeFromSfM(int tc, double& out_zmin, double& ou const Point3d point(landmark.X(0), landmark.X(1), landmark.X(2)); // no tc observation - if (landmark.observations.find(tcViewId) == landmark.observations.end()) + if (landmark.getObservations().find(tcViewId) == landmark.getObservations().end()) continue; // find rc observation - const auto it = landmark.observations.find(rcViewId); + const auto it = landmark.getObservations().find(rcViewId); // no rc observation - if (it == landmark.observations.end()) + if (it == landmark.getObservations().end()) continue; // get rc 2d observation diff --git a/src/aliceVision/depthMap/volumeIO.cpp b/src/aliceVision/depthMap/volumeIO.cpp index bb917f55cc..72ea5ce7f5 100644 --- a/src/aliceVision/depthMap/volumeIO.cpp +++ b/src/aliceVision/depthMap/volumeIO.cpp @@ -183,7 +183,7 @@ void exportSimilarityVolume(const CudaHostMemoryHeap& in_volumeSim_hmh, continue; const rgb c = getRGBFromJetColorMap(simValue / maxValue); pointCloud.getLandmarks()[landmarkId] = sfmData::Landmark( - Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b)); + Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b)); ++landmarkId; } @@ -235,7 +235,7 @@ void exportSimilarityVolumeCross(const CudaHostMemoryHeap& in_volumeSim const rgb c = getRGBFromJetColorMap(simValue / maxValue); pointCloud.getLandmarks()[landmarkId] = sfmData::Landmark( - Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b)); + Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b)); ++landmarkId; } @@ -293,7 +293,7 @@ void exportSimilarityVolumeCross(const CudaHostMemoryHeap& in_vol const rgb c = getRGBFromJetColorMap(simValue / maxValue); pointCloud.getLandmarks()[landmarkId] = sfmData::Landmark( - Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b)); + Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b)); ++landmarkId; } @@ -365,7 +365,7 @@ void exportSimilarityVolumeTopographicCut(const CudaHostMemoryHeap& in_ const rgb c = getRGBFromJetColorMap(simValueNorm); pointCloud.getLandmarks()[landmarkId] = - sfmData::Landmark(Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b)); + sfmData::Landmark(Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b)); ++landmarkId; } @@ -430,7 +430,7 @@ void exportSimilarityVolumeTopographicCut(const CudaHostMemoryHeap& in_volumeSim_hmh, float4 colorValue = *get3DBufferAt_h(in_volumeSim_hmh.getBuffer(), spitch, pitch, vx, vy, vz); const rgb c = float4_to_rgb(colorValue); // TODO: convert Lab color into sRGB color pointCloud.getLandmarks()[landmarkId] = sfmData::Landmark( - Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b)); + Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b)); ++landmarkId; } diff --git a/src/aliceVision/fuseCut/DelaunayGraphCut.cpp b/src/aliceVision/fuseCut/DelaunayGraphCut.cpp index 67fac2d19b..edfd9a34a0 100644 --- a/src/aliceVision/fuseCut/DelaunayGraphCut.cpp +++ b/src/aliceVision/fuseCut/DelaunayGraphCut.cpp @@ -775,10 +775,10 @@ void DelaunayGraphCut::addPointsFromSfM(const Point3d hexah[8], const StaticVect { *vCoordsIt = p; - vAttrIt->nrc = landmark.observations.size(); + vAttrIt->nrc = landmark.getObservations().size(); vAttrIt->cams.reserve(vAttrIt->nrc); - for (const auto& observationPair : landmark.observations) + for (const auto& observationPair : landmark.getObservations()) vAttrIt->cams.push_back(_mp.getIndexFromViewId(observationPair.first)); vAttrIt->pixSize = _mp.getCamsMinPixelSize(p, vAttrIt->cams); diff --git a/src/aliceVision/fuseCut/DelaunayGraphCut_test.cpp b/src/aliceVision/fuseCut/DelaunayGraphCut_test.cpp index 1ef761648c..056bbbad5c 100644 --- a/src/aliceVision/fuseCut/DelaunayGraphCut_test.cpp +++ b/src/aliceVision/fuseCut/DelaunayGraphCut_test.cpp @@ -200,7 +200,7 @@ SfMData generateSfm(const NViewDatasetConfigurator& config, const size_t size, c for (int j = 0; j < camsPts.size(); ++j) { const Vec2 pt = projectedPtsPerCam[j].col(i); - landmark.observations[j] = Observation(pt, i, unknownScale); + landmark.getObservations()[j] = Observation(pt, i, unknownScale); } sfm_data.getLandmarks()[i] = landmark; } diff --git a/src/aliceVision/fuseCut/Fuser.cpp b/src/aliceVision/fuseCut/Fuser.cpp index a72fb979ab..7df157f72d 100644 --- a/src/aliceVision/fuseCut/Fuser.cpp +++ b/src/aliceVision/fuseCut/Fuser.cpp @@ -360,7 +360,7 @@ float Fuser::computeAveragePixelSizeInHexahedron(Point3d* hexah, const sfmData:: const sfmData::Landmark& landmark = landmarkPair.second; const Point3d p(landmark.X(0), landmark.X(1), landmark.X(2)); - for (const auto& observationPair : landmark.observations) + for (const auto& observationPair : landmark.getObservations()) { const IndexT viewId = observationPair.first; @@ -506,14 +506,14 @@ void Fuser::divideSpaceFromDepthMaps(Point3d* hexah, float& minPixSize) bool checkLandmarkMinObservationAngle(const sfmData::SfMData& sfmData, const sfmData::Landmark& landmark, float minObservationAngle) { - for (const auto& observationPairI : landmark.observations) + for (const auto& observationPairI : landmark.getObservations()) { const IndexT I = observationPairI.first; const sfmData::View& viewI = *(sfmData.getViews().at(I)); const geometry::Pose3 poseI = sfmData.getPose(viewI).getTransform(); const camera::IntrinsicBase* intrinsicPtrI = sfmData.getIntrinsicPtr(viewI.getIntrinsicId()); - for (const auto& observationPairJ : landmark.observations) + for (const auto& observationPairJ : landmark.getObservations()) { const IndexT J = observationPairJ.first; @@ -562,7 +562,7 @@ void Fuser::divideSpaceFromSfM(const sfmData::SfMData& sfmData, Point3d* hexah, const sfmData::Landmark& landmark = landmarkPair.second; // check number of observations - if (landmark.observations.size() < minObservations) + if (landmark.getObservations().size() < minObservations) continue; // check angle between observations diff --git a/src/aliceVision/localization/CCTagLocalizer.cpp b/src/aliceVision/localization/CCTagLocalizer.cpp index c103ed1df8..6a0bf73376 100644 --- a/src/aliceVision/localization/CCTagLocalizer.cpp +++ b/src/aliceVision/localization/CCTagLocalizer.cpp @@ -77,7 +77,7 @@ bool CCTagLocalizer::loadReconstructionDescriptors(const sfmData::SfMData& sfm_d IndexT trackId = landmarkValue.first; const sfmData::Landmark& landmark = landmarkValue.second; - for (const auto& obs : landmark.observations) + for (const auto& obs : landmark.getObservations()) { const IndexT viewId = obs.first; const sfmData::Observation& obs2d = obs.second; diff --git a/src/aliceVision/localization/VoctreeLocalizer.cpp b/src/aliceVision/localization/VoctreeLocalizer.cpp index 8139ebfe43..1fd2bb0919 100644 --- a/src/aliceVision/localization/VoctreeLocalizer.cpp +++ b/src/aliceVision/localization/VoctreeLocalizer.cpp @@ -286,7 +286,7 @@ bool VoctreeLocalizer::initDatabase(const std::string& vocTreeFilepath, const st IndexT trackId = landmarkValue.first; const sfmData::Landmark& landmark = landmarkValue.second; - for (const auto& obs : landmark.observations) + for (const auto& obs : landmark.getObservations()) { const IndexT viewId = obs.first; const sfmData::Observation& obs2d = obs.second; diff --git a/src/aliceVision/localization/optimization.cpp b/src/aliceVision/localization/optimization.cpp index c1bc7dc797..42510fcaad 100644 --- a/src/aliceVision/localization/optimization.cpp +++ b/src/aliceVision/localization/optimization.cpp @@ -148,11 +148,11 @@ bool refineSequence(std::vector& vec_localizationResult, assert(landmark.descType == match.descType); // normally there should be no other features already associated to this // 3D point in this view - if (landmark.observations.count(viewID) != 0) + if (landmark.getObservations().count(viewID) != 0) { // this is weird but it could happen when two features are really close to each other (?) ALICEVISION_LOG_DEBUG("Point 3D " << match.landmarkId << " has multiple features " - << "in the same view " << viewID << ", current size of obs: " << landmark.observations.size()); + << "in the same view " << viewID << ", current size of obs: " << landmark.getObservations().size()); ALICEVISION_LOG_DEBUG("its associated features are: "); for (std::size_t i = 0; i < matches.size(); ++i) { @@ -169,7 +169,7 @@ bool refineSequence(std::vector& vec_localizationResult, } // the 3D point exists already, add the observation - landmark.observations[viewID] = sfmData::Observation(feature, match.featId, unknownScale); + landmark.getObservations()[viewID] = sfmData::Observation(feature, match.featId, unknownScale); } else { @@ -177,7 +177,7 @@ bool refineSequence(std::vector& vec_localizationResult, sfmData::Landmark newLandmark; newLandmark.descType = match.descType; newLandmark.X = currResult.getPt3D().col(idx); - newLandmark.observations[viewID] = sfmData::Observation(feature, match.featId, unknownScale); + newLandmark.getObservations()[viewID] = sfmData::Observation(feature, match.featId, unknownScale); tinyScene.getLandmarks()[match.landmarkId] = std::move(newLandmark); } } @@ -188,7 +188,7 @@ bool refineSequence(std::vector& vec_localizationResult, // sfmData::Landmarks &landmarks = tinyScene.getLandmarks(); // for(sfmData::Landmarks::iterator it = landmarks.begin(), ite = landmarks.end(); it != ite;) // { - // if(it->second.observations.size() < 5) + // if(it->second.getObservations().size() < 5) // { // it = landmarks.erase(it); // } @@ -205,15 +205,15 @@ bool refineSequence(std::vector& vec_localizationResult, std::size_t maxObs = 0; for (const auto landmark : tinyScene.getLandmarks()) { - if (landmark.second.observations.size() > maxObs) - maxObs = landmark.second.observations.size(); + if (landmark.second.getObservations().size() > maxObs) + maxObs = landmark.second.getObservations().size(); } namespace bacc = boost::accumulators; bacc::accumulator_set> stats; std::vector hist(maxObs + 1, 0); for (const auto landmark : tinyScene.getLandmarks()) { - const std::size_t nobs = landmark.second.observations.size(); + const std::size_t nobs = landmark.second.getObservations().size(); assert(nobs < hist.size()); stats(nobs); hist[nobs]++; @@ -250,7 +250,7 @@ bool refineSequence(std::vector& vec_localizationResult, for (; iter != endIter;) { - if (iter->second.observations.size() < minPointVisibility) + if (iter->second.getObservations().size() < minPointVisibility) { iter = landmarks.erase(iter); } diff --git a/src/aliceVision/multiview/resection/resectionLORansac_test.cpp b/src/aliceVision/multiview/resection/resectionLORansac_test.cpp index 867ebe2f4b..c0335438f7 100644 --- a/src/aliceVision/multiview/resection/resectionLORansac_test.cpp +++ b/src/aliceVision/multiview/resection/resectionLORansac_test.cpp @@ -57,7 +57,7 @@ bool refinePoseAsItShouldbe(const Mat& pt3D, const size_t idx = vec_inliers[i]; Landmark landmark; landmark.X = pt3D.col(idx); - landmark.observations[0] = Observation(pt2D.col(idx), UndefinedIndexT, unknownScale); + landmark.getObservations()[0] = Observation(pt2D.col(idx), UndefinedIndexT, unknownScale); sfm_data.getLandmarks()[i] = std::move(landmark); } diff --git a/src/aliceVision/mvsUtils/MultiViewParams.cpp b/src/aliceVision/mvsUtils/MultiViewParams.cpp index 8650a81300..714f5f1423 100644 --- a/src/aliceVision/mvsUtils/MultiViewParams.cpp +++ b/src/aliceVision/mvsUtils/MultiViewParams.cpp @@ -528,7 +528,7 @@ StaticVector MultiViewParams::findNearestCamsFromLandmarks(int rc, int nbNe for (const auto& landmarkPair : _sfmData.getLandmarks()) { - const auto& observations = landmarkPair.second.observations; + const auto& observations = landmarkPair.second.getObservations(); auto viewObsIt = observations.find(viewId); if (viewObsIt == observations.end()) @@ -604,7 +604,7 @@ std::vector MultiViewParams::findTileNearestCams(int rc, int nbNearestCams, for (const auto& landmarkPair : sfmData.getLandmarks()) { - const auto& observations = landmarkPair.second.observations; + const auto& observations = landmarkPair.second.getObservations(); auto viewObsIt = observations.find(viewId); diff --git a/src/aliceVision/photometricStereo/normalIntegration.cpp b/src/aliceVision/photometricStereo/normalIntegration.cpp index d065f14926..b6ae41250a 100644 --- a/src/aliceVision/photometricStereo/normalIntegration.cpp +++ b/src/aliceVision/photometricStereo/normalIntegration.cpp @@ -543,7 +543,7 @@ void adjustScale(const sfmData::SfMData& sfmData, image::Image& initDepth const sfmData::Landmark& currentLandmark = landmarks.at(currentLandmarkIndex); knownDepths(i) = pose.depth(currentLandmark.X); - sfmData::Observation observationInCurrentPicture = currentLandmark.observations.at(viewID); + sfmData::Observation observationInCurrentPicture = currentLandmark.getObservations().at(viewID); int rowInd = observationInCurrentPicture.getY(); int colInd = observationInCurrentPicture.getX(); @@ -576,7 +576,7 @@ void getZ0FromLandmarks(const sfmData::SfMData& sfmData, { size_t currentLandmarkIndex = visibleLandmarks.at(i); const sfmData::Landmark& currentLandmark = landmarks.at(currentLandmarkIndex); - sfmData::Observation observationInCurrentPicture = currentLandmark.observations.at(viewID); + sfmData::Observation observationInCurrentPicture = currentLandmark.getObservations().at(viewID); int rowInd = observationInCurrentPicture.getY(); int colInd = observationInCurrentPicture.getX(); diff --git a/src/aliceVision/sfm/FrustumFilter.cpp b/src/aliceVision/sfm/FrustumFilter.cpp index 3de5244e20..f6a7ca4925 100644 --- a/src/aliceVision/sfm/FrustumFilter.cpp +++ b/src/aliceVision/sfm/FrustumFilter.cpp @@ -169,7 +169,7 @@ void FrustumFilter::init_z_near_z_far_depth(const sfmData::SfMData& sfmData, con { const sfmData::Landmark& landmark = itL->second; const Vec3& X = landmark.X; - for (sfmData::Observations::const_iterator iterO = landmark.observations.begin(); iterO != landmark.observations.end(); ++iterO) + for (sfmData::Observations::const_iterator iterO = landmark.getObservations().begin(); iterO != landmark.getObservations().end(); ++iterO) { const IndexT id_view = iterO->first; const sfmData::Observation& ob = iterO->second; diff --git a/src/aliceVision/sfm/LocalBundleAdjustmentGraph.cpp b/src/aliceVision/sfm/LocalBundleAdjustmentGraph.cpp index 5e40162792..0fd5fc6b75 100644 --- a/src/aliceVision/sfm/LocalBundleAdjustmentGraph.cpp +++ b/src/aliceVision/sfm/LocalBundleAdjustmentGraph.cpp @@ -413,7 +413,7 @@ void LocalBundleAdjustmentGraph::convertDistancesToStates(sfmData::SfMData& sfmD for (auto& itLandmark : sfmData.getLandmarks()) { const IndexT landmarkId = itLandmark.first; - const sfmData::Observations& observations = itLandmark.second.observations; + const sfmData::Observations& observations = itLandmark.second.getObservations(); assert(observations.size() >= 2); @@ -475,7 +475,7 @@ std::vector LocalBundleAdjustmentGraph::getNewEdges(const sfmData::SfMData // retrieve the common track Ids for (IndexT landmarkId : newViewLandmarks) { - for (const auto& observations : sfmData.getLandmarks().at(landmarkId).observations) + for (const auto& observations : sfmData.getLandmarks().at(landmarkId).getObservations()) { if (observations.first == viewId) continue; // do not compare an observation with itself diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index 06c4d06d41..bbbfb936df 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -806,7 +806,7 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat _allParametersBlocks.push_back(landmarkBlockPtr); // iterate over 2D observation associated to the 3D landmark - for (const auto& observationPair : landmark.observations) + for (const auto& observationPair : landmark.getObservations()) { const sfmData::View& view = sfmData.getView(observationPair.first); const sfmData::Observation& observation = observationPair.second; diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp index e92be38a8e..70a4226ba6 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentSymbolicCeres.cpp @@ -491,7 +491,7 @@ void BundleAdjustmentSymbolicCeres::addLandmarksToProblem(const sfmData::SfMData _allParametersBlocks.push_back(landmarkBlockPtr); // iterate over 2D observation associated to the 3D landmark - for (const auto& observationPair : landmark.observations) + for (const auto& observationPair : landmark.getObservations()) { const sfmData::View& view = sfmData.getView(observationPair.first); const sfmData::Observation& observation = observationPair.second; diff --git a/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp b/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp index ba908a9230..c1bc6f89b0 100644 --- a/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp +++ b/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp @@ -154,12 +154,12 @@ BOOST_AUTO_TEST_CASE(LOCAL_BUNDLE_ADJUSTMENT_EffectiveMinimization_Pinhole_Camer // | | => / \ / \ / \ // v1 - v2 v0 v1 v2 v3 // removing adequate observations: - sfmData.getLandmarks().at(0).observations.erase(2); - sfmData.getLandmarks().at(0).observations.erase(3); - sfmData.getLandmarks().at(1).observations.erase(0); - sfmData.getLandmarks().at(1).observations.erase(3); - sfmData.getLandmarks().at(2).observations.erase(0); - sfmData.getLandmarks().at(2).observations.erase(1); + sfmData.getLandmarks().at(0).getObservations().erase(2); + sfmData.getLandmarks().at(0).getObservations().erase(3); + sfmData.getLandmarks().at(1).getObservations().erase(0); + sfmData.getLandmarks().at(1).getObservations().erase(3); + sfmData.getLandmarks().at(2).getObservations().erase(0); + sfmData.getLandmarks().at(2).getObservations().erase(1); // lock common intrinsic // if it's not locked, all views will have a distance of 1 as all views share a common intrinsic. @@ -248,7 +248,7 @@ double RMSE(const SfMData& sfm_data) std::vector vec; for (Landmarks::const_iterator iterTracks = sfm_data.getLandmarks().begin(); iterTracks != sfm_data.getLandmarks().end(); ++iterTracks) { - const Observations& observations = iterTracks->second.observations; + const Observations& observations = iterTracks->second.getObservations(); for (Observations::const_iterator itObs = observations.begin(); itObs != observations.end(); ++itObs) { const View* view = sfm_data.getViews().find(itObs->first)->second.get(); @@ -315,7 +315,7 @@ SfMData getInputScene(const NViewDataSet& d, const NViewDatasetConfigurator& con pt(0) += rand() / RAND_MAX - .5; pt(1) += rand() / RAND_MAX - .5; - landmark.observations[j] = Observation(pt, i, unknownScale); + landmark.getObservations()[j] = Observation(pt, i, unknownScale); } sfm_data.getLandmarks()[i] = landmark; } diff --git a/src/aliceVision/sfm/generateReport.cpp b/src/aliceVision/sfm/generateReport.cpp index 6e695390ca..3a9529eebb 100644 --- a/src/aliceVision/sfm/generateReport.cpp +++ b/src/aliceVision/sfm/generateReport.cpp @@ -26,7 +26,7 @@ bool generateSfMReport(const sfmData::SfMData& sfmData, const std::string& htmlF HashMap> residuals_per_view; for (sfmData::Landmarks::const_iterator iterTracks = sfmData.getLandmarks().begin(); iterTracks != sfmData.getLandmarks().end(); ++iterTracks) { - const sfmData::Observations& observations = iterTracks->second.observations; + const sfmData::Observations& observations = iterTracks->second.getObservations(); for (sfmData::Observations::const_iterator itObs = observations.begin(); itObs != observations.end(); ++itObs) { const sfmData::View* view = sfmData.getViews().at(itObs->first).get(); diff --git a/src/aliceVision/sfm/pipeline/ReconstructionEngine.cpp b/src/aliceVision/sfm/pipeline/ReconstructionEngine.cpp index 46b867d042..b79d858d82 100644 --- a/src/aliceVision/sfm/pipeline/ReconstructionEngine.cpp +++ b/src/aliceVision/sfm/pipeline/ReconstructionEngine.cpp @@ -49,13 +49,13 @@ void retrieveMarkersId(sfmData::SfMData& sfmData) for (auto& landmarkIt : sfmData.getLandmarks()) { auto& landmark = landmarkIt.second; - if (landmark.observations.empty()) + if (landmark.getObservations().empty()) continue; if (markerDescTypes_set.find(landmark.descType) == markerDescTypes_set.end()) continue; landmark.rgb = image::BLACK; - const auto obs = landmark.observations.begin(); + const auto obs = landmark.getObservations().begin(); const feature::Regions& regions = regionPerView.getRegions(obs->first, landmark.descType); const feature::CCTAG_Regions* cctagRegions = dynamic_cast(®ions); const feature::APRILTAG_Regions* apriltagRegions = dynamic_cast(®ions); diff --git a/src/aliceVision/sfm/pipeline/RigSequence.cpp b/src/aliceVision/sfm/pipeline/RigSequence.cpp index 4b6cf77758..cce2c96491 100644 --- a/src/aliceVision/sfm/pipeline/RigSequence.cpp +++ b/src/aliceVision/sfm/pipeline/RigSequence.cpp @@ -64,9 +64,9 @@ double computeCameraScore(const SfMData& sfmData, const track::TracksPerView& tr { const Landmark& landmark = sfmData.getLandmarks().at(static_cast(landmarkId)); - sfmData::Observations::const_iterator itObs = landmark.observations.find(viewId); + sfmData::Observations::const_iterator itObs = landmark.getObservations().find(viewId); - if (itObs != landmark.observations.end()) + if (itObs != landmark.getObservations().end()) { const Vec2 residual = intrinsic->residual(pose, landmark.X.homogeneous(), itObs->second.getCoordinates()); score += std::min(1.0 / residual.norm(), 4.0); diff --git a/src/aliceVision/sfm/pipeline/global/ReconstructionEngine_globalSfM.cpp b/src/aliceVision/sfm/pipeline/global/ReconstructionEngine_globalSfM.cpp index 5fc306c0bf..60118240c2 100644 --- a/src/aliceVision/sfm/pipeline/global/ReconstructionEngine_globalSfM.cpp +++ b/src/aliceVision/sfm/pipeline/global/ReconstructionEngine_globalSfM.cpp @@ -300,7 +300,7 @@ bool ReconstructionEngine_globalSfM::Compute_Initial_Structure(matching::Pairwis const Track& track = itTracks->second; Landmark& newLandmark = structure[idx]; newLandmark.descType = track.descType; - Observations& obs = newLandmark.observations; + Observations& obs = newLandmark.getObservations(); for (Track::FeatureIdPerView::const_iterator it = track.featPerView.begin(); it != track.featPerView.end(); ++it) { const size_t imaIndex = it->first; @@ -601,7 +601,7 @@ void ReconstructionEngine_globalSfM::Compute_Relative_Rotations(rotationAveragin obs[view_J->getViewId()] = Observation(x2_, match._j, scaleJ); Landmark& newLandmark = landmarks[landmarkId++]; newLandmark.descType = descType; - newLandmark.observations = obs; + newLandmark.getObservations() = obs; newLandmark.X = X; } } diff --git a/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp b/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp index d3a80fb023..13c5bd4d54 100644 --- a/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp +++ b/src/aliceVision/sfm/pipeline/localization/SfMLocalizer.cpp @@ -207,7 +207,7 @@ bool SfMLocalizer::RefinePose(camera::IntrinsicBase* intrinsics, const std::size_t idx = matchingData.vec_inliers[i]; sfmData::Landmark landmark; landmark.X = matchingData.pt3D.col(idx); - landmark.observations[0] = sfmData::Observation(matchingData.pt2D.col(idx), UndefinedIndexT, unknownScale); // TODO-SCALE + landmark.getObservations()[0] = sfmData::Observation(matchingData.pt2D.col(idx), UndefinedIndexT, unknownScale); // TODO-SCALE tinyScene.getLandmarks()[i] = std::move(landmark); } diff --git a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp index 489a99cf01..5d880b6469 100644 --- a/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp +++ b/src/aliceVision/sfm/pipeline/panorama/ReconstructionEngine_panorama.cpp @@ -932,8 +932,8 @@ bool ReconstructionEngine_panorama::buildLandmarks() // Store landmark Landmark l; l.descType = c.descType; - l.observations[c.ViewFirst] = c.ObservationFirst; - l.observations[c.ViewSecond] = c.ObservationSecond; + l.getObservations()[c.ViewFirst] = c.ObservationFirst; + l.getObservations()[c.ViewSecond] = c.ObservationSecond; l.X = (wpt1 + wpt2) * 0.5; _sfmData.getLandmarks()[count++] = l; diff --git a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp index 0757115755..53108b23ba 100644 --- a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp +++ b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp @@ -376,7 +376,7 @@ void ReconstructionEngine_sequentialSfM::registerChanges(std::set& linke // Check that this view is a registered observation of this landmark (confirmed track) const Landmark& l = landmarks.at(idTrack); - if (l.observations.find(id) == l.observations.end()) + if (l.getObservations().find(id) == l.getObservations().end()) { continue; } @@ -424,8 +424,8 @@ void ReconstructionEngine_sequentialSfM::remapLandmarkIdsToTrackIds() for (const auto& landmarkPair : landmarks) { const IndexT landmarkId = landmarkPair.first; - const IndexT firstViewId = landmarkPair.second.observations.begin()->first; - const IndexT firstFeatureId = landmarkPair.second.observations.begin()->second.getFeatureId(); + const IndexT firstViewId = landmarkPair.second.getObservations().begin()->first; + const IndexT firstFeatureId = landmarkPair.second.getObservations().begin()->second.getFeatureId(); const feature::EImageDescriberType descType = landmarkPair.second.descType; obsToLandmark.emplace(ObsKey(firstViewId, firstFeatureId, descType), landmarkId); @@ -953,7 +953,7 @@ void ReconstructionEngine_sequentialSfM::exportStatistics(double reconstructionT std::map obsHistogram; for (const auto& iterTracks : _sfmData.getLandmarks()) { - const Observations& obs = iterTracks.second.observations; + const Observations& obs = iterTracks.second.getObservations(); if (obsHistogram.count(obs.size())) obsHistogram[obs.size()]++; else @@ -1673,7 +1673,7 @@ void ReconstructionEngine_sequentialSfM::updateScene(const IndexT viewIndex, con ? 0.0 : _featuresPerView->getFeatures(viewIndex, landmark.descType)[idFeat].scale(); // Inlier, add the point to the reconstructed track - landmark.observations[viewIndex] = Observation(x, idFeat, scale); + landmark.getObservations()[viewIndex] = Observation(x, idFeat, scale); } } } @@ -1922,7 +1922,7 @@ void ReconstructionEngine_sequentialSfM::triangulate_multiViewsLORANSAC(SfMData& const Vec2 x = _featuresPerView->getFeatures(viewId, track.descType)[track.featPerView.at(viewId).featureId].coords().cast(); const feature::PointFeature& p = _featuresPerView->getFeatures(viewId, track.descType)[track.featPerView.at(viewId).featureId]; const double scale = (_params.featureConstraint == EFeatureConstraint::BASIC) ? 0.0 : p.scale(); - landmark.observations[viewId] = Observation(x, track.featPerView.at(viewId).featureId, scale); + landmark.getObservations()[viewId] = Observation(x, track.featPerView.at(viewId).featureId, scale); } #pragma omp critical { @@ -2024,7 +2024,7 @@ void ReconstructionEngine_sequentialSfM::triangulate_2Views(SfMData& scene, #pragma omp critical { Landmark& landmark = scene.getLandmarks().at(trackId); - if (landmark.observations.count(I) == 0) + if (landmark.getObservations().count(I) == 0) { const Vec2 residual = camI->residual(poseI, landmark.X.homogeneous(), xI); // TODO: scale in residual @@ -2034,11 +2034,11 @@ void ReconstructionEngine_sequentialSfM::triangulate_2Views(SfMData& scene, if (poseI.depth(landmark.X) > 0 && residual.norm() < std::max(4.0, acThreshold)) { const double scale = (_params.featureConstraint == EFeatureConstraint::BASIC) ? 0.0 : featI.scale(); - landmark.observations[I] = Observation(xI, track.featPerView.at(I).featureId, scale); + landmark.getObservations()[I] = Observation(xI, track.featPerView.at(I).featureId, scale); ++extented_track; } } - if (landmark.observations.count(J) == 0) + if (landmark.getObservations().count(J) == 0) { const Vec2 residual = camJ->residual(poseJ, landmark.X.homogeneous(), xJ); const auto& acThresholdIt = _map_ACThreshold.find(J); @@ -2047,7 +2047,7 @@ void ReconstructionEngine_sequentialSfM::triangulate_2Views(SfMData& scene, if (poseJ.depth(landmark.X) > 0 && residual.norm() < std::max(4.0, acThreshold)) { const double scale = (_params.featureConstraint == EFeatureConstraint::BASIC) ? 0.0 : featJ.scale(); - landmark.observations[J] = Observation(xJ, track.featPerView.at(J).featureId, scale); + landmark.getObservations()[J] = Observation(xJ, track.featPerView.at(J).featureId, scale); ++extented_track; } } @@ -2097,8 +2097,8 @@ void ReconstructionEngine_sequentialSfM::triangulate_2Views(SfMData& scene, const double scaleI = (_params.featureConstraint == EFeatureConstraint::BASIC) ? 0.0 : featI.scale(); const double scaleJ = (_params.featureConstraint == EFeatureConstraint::BASIC) ? 0.0 : featJ.scale(); - landmark.observations[I] = Observation(xI, track.featPerView.at(I).featureId, scaleI); - landmark.observations[J] = Observation(xJ, track.featPerView.at(J).featureId, scaleJ); + landmark.getObservations()[I] = Observation(xI, track.featPerView.at(I).featureId, scaleI); + landmark.getObservations()[J] = Observation(xJ, track.featPerView.at(J).featureId, scaleJ); ++new_added_track; } // critical diff --git a/src/aliceVision/sfm/pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp b/src/aliceVision/sfm/pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp index 19cc3619cb..84c29e8b92 100644 --- a/src/aliceVision/sfm/pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp +++ b/src/aliceVision/sfm/pipeline/structureFromKnownPoses/StructureEstimationFromKnownPoses.cpp @@ -275,7 +275,7 @@ void StructureEstimationFromKnownPoses::triangulate(SfMData& sfmData, { const track::Track& track = itTracks->second; structure[idx] = Landmark(track.descType); - Observations& observations = structure.at(idx).observations; + Observations& observations = structure.at(idx).getObservations(); for (auto it = track.featPerView.begin(); it != track.featPerView.end(); ++it) { const size_t imaIndex = it->first; diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp index a24cd75a24..0428045fb1 100644 --- a/src/aliceVision/sfm/sfmFilters.cpp +++ b/src/aliceVision/sfm/sfmFilters.cpp @@ -26,7 +26,7 @@ IndexT RemoveOutliers_PixelResidualError(sfmData::SfMData& sfmData, while (iterTracks != sfmData.getLandmarks().end()) { - sfmData::Observations& observations = iterTracks->second.observations; + sfmData::Observations& observations = iterTracks->second.getObservations(); sfmData::Observations::iterator itObs = observations.begin(); while (itObs != observations.end()) @@ -75,7 +75,7 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc #pragma omp parallel for for (int landmarkIndex = 0; landmarkIndex < v_keys.size(); ++landmarkIndex) { - const sfmData::Observations& observations = sfmData.getLandmarks().at(v_keys[landmarkIndex]).observations; + const sfmData::Observations& observations = sfmData.getLandmarks().at(v_keys[landmarkIndex]).getObservations(); // create matrix for observation directions from camera to point Mat3X viewDirections(3, observations.size()); @@ -167,7 +167,7 @@ bool eraseUnstablePoses(sfmData::SfMData& sfmData, const IndexT min_points_per_p // Count occurrence of the poses in the Landmark observations for (sfmData::Landmarks::const_iterator itLandmarks = landmarks.begin(); itLandmarks != landmarks.end(); ++itLandmarks) { - const sfmData::Observations& observations = itLandmarks->second.observations; + const sfmData::Observations& observations = itLandmarks->second.getObservations(); for (sfmData::Observations::const_iterator itObs = observations.begin(); itObs != observations.end(); ++itObs) { const IndexT viewId = itObs->first; @@ -229,7 +229,7 @@ bool eraseObservationsWithMissingPoses(sfmData::SfMData& sfmData, const IndexT m while (itLandmarks != sfmData.getLandmarks().end()) { - sfmData::Observations& observations = itLandmarks->second.observations; + sfmData::Observations& observations = itLandmarks->second.getObservations(); sfmData::Observations::iterator itObs = observations.begin(); while (itObs != observations.end()) diff --git a/src/aliceVision/sfm/sfmStatistics.cpp b/src/aliceVision/sfm/sfmStatistics.cpp index e011d58b3d..7b36556a59 100644 --- a/src/aliceVision/sfm/sfmStatistics.cpp +++ b/src/aliceVision/sfm/sfmStatistics.cpp @@ -37,7 +37,7 @@ void computeResidualsHistogram(const sfmData::SfMData& sfmData, for (const auto& track : sfmData.getLandmarks()) { - const aliceVision::sfmData::Observations& observations = track.second.observations; + const aliceVision::sfmData::Observations& observations = track.second.getObservations(); for (const auto& obs : observations) { if (!specificViews.empty()) @@ -92,7 +92,7 @@ void computeObservationsLengthsHistogram(const sfmData::SfMData& sfmData, for (const auto& landmark : sfmData.getLandmarks()) { - const aliceVision::sfmData::Observations& observations = landmark.second.observations; + const aliceVision::sfmData::Observations& observations = landmark.second.getObservations(); if (!specificViews.empty()) { int nbObsSpecificViews = 0; @@ -144,7 +144,7 @@ void computeLandmarksPerViewHistogram(const sfmData::SfMData& sfmData, BoxStats< for (const auto& landmark : sfmData.getLandmarks()) { - for (const auto& obsIt : landmark.second.observations) + for (const auto& obsIt : landmark.second.getObservations()) { const auto& viewId = obsIt.first; auto it = nbLandmarksPerView.find(viewId); @@ -184,7 +184,7 @@ void computeLandmarksPerView(const sfmData::SfMData& sfmData, std::vector& for (const auto& landmark : sfmData.getLandmarks()) { - const aliceVision::sfmData::Observations& observations = landmark.second.observations; + const aliceVision::sfmData::Observations& observations = landmark.second.getObservations(); for (const auto& obs : observations) { auto it = nbLandmarksPerView.find(obs.first); @@ -283,7 +283,7 @@ void computeScaleHistogram(const sfmData::SfMData& sfmData, vec_scaleObservations.reserve(sfmData.getLandmarks().size()); for (const auto& landmark : sfmData.getLandmarks()) { - const aliceVision::sfmData::Observations& observations = landmark.second.observations; + const aliceVision::sfmData::Observations& observations = landmark.second.getObservations(); for (const auto& obs : observations) { @@ -334,7 +334,7 @@ void computeResidualsPerView(const sfmData::SfMData& sfmData, for (const auto& landmark : sfmData.getLandmarks()) { - const aliceVision::sfmData::Observations& observations = landmark.second.observations; + const aliceVision::sfmData::Observations& observations = landmark.second.getObservations(); for (const auto& obs : observations) { const sfmData::View& view = sfmData.getView(obs.first); @@ -397,7 +397,7 @@ void computeObservationsLengthsPerView(const sfmData::SfMData& sfmData, for (const auto& landmark : sfmData.getLandmarks()) { - const aliceVision::sfmData::Observations& observations = landmark.second.observations; + const aliceVision::sfmData::Observations& observations = landmark.second.getObservations(); for (const auto& obs : observations) { observationLengthsPerView[obs.first].push_back(observations.size()); diff --git a/src/aliceVision/sfm/sfmTriangulation.cpp b/src/aliceVision/sfm/sfmTriangulation.cpp index aa7437f2c0..b4d9d3f220 100644 --- a/src/aliceVision/sfm/sfmTriangulation.cpp +++ b/src/aliceVision/sfm/sfmTriangulation.cpp @@ -46,7 +46,7 @@ void StructureComputation_blind::triangulate(sfmData::SfMData& sfmData, std::mt1 } // Triangulate each landmark multiview::Triangulation trianObj; - const sfmData::Observations& observations = iterTracks->second.observations; + const sfmData::Observations& observations = iterTracks->second.getObservations(); for (const auto& itObs : observations) { const sfmData::View* view = sfmData.getViews().at(itObs.first).get(); @@ -126,7 +126,7 @@ void StructureComputation_robust::robust_triangulation(sfmData::SfMData& sfmData ++(progressDisplay); } Vec3 X; - if (robust_triangulation(sfmData, iterTracks->second.observations, randomNumberGenerator, X)) + if (robust_triangulation(sfmData, iterTracks->second.getObservations(), randomNumberGenerator, X)) { iterTracks->second.X = X; } diff --git a/src/aliceVision/sfm/utils/alignment.cpp b/src/aliceVision/sfm/utils/alignment.cpp index 5116a7fbac..a7b9584a67 100644 --- a/src/aliceVision/sfm/utils/alignment.cpp +++ b/src/aliceVision/sfm/utils/alignment.cpp @@ -1028,7 +1028,7 @@ void computeNewCoordinateSystemGroundAuto(const sfmData::SfMData& sfmData, Vec3& for (auto& plandmark : sfmData.getLandmarks()) { // Filter out landmarks with not enough observations - if (plandmark.second.observations.size() < 3) + if (plandmark.second.getObservations().size() < 3) { continue; } @@ -1037,7 +1037,7 @@ void computeNewCoordinateSystemGroundAuto(const sfmData::SfMData& sfmData, Vec3& // This filtering step assumes that cameras should not be underneath the ground level const Vec3 X = plandmark.second.X; bool foundUnder = false; - for (const auto& pObs : plandmark.second.observations) + for (const auto& pObs : plandmark.second.getObservations()) { const IndexT viewId = pObs.first; const Vec3 camCenter = sfmData.getPose(sfmData.getView(viewId)).getTransform().center(); diff --git a/src/aliceVision/sfm/utils/alignment_test.cpp b/src/aliceVision/sfm/utils/alignment_test.cpp index 742658623e..44e95511e1 100644 --- a/src/aliceVision/sfm/utils/alignment_test.cpp +++ b/src/aliceVision/sfm/utils/alignment_test.cpp @@ -187,7 +187,7 @@ SfMData getInputScene(const NViewDataSet& d, const NViewDatasetConfigurator& con pt(0) += rand() / RAND_MAX - .5; pt(1) += rand() / RAND_MAX - .5; - landmark.observations[j] = Observation(pt, i, unknownScale); + landmark.getObservations()[j] = Observation(pt, i, unknownScale); } sfm_data.getLandmarks()[i] = landmark; } diff --git a/src/aliceVision/sfm/utils/statistics.cpp b/src/aliceVision/sfm/utils/statistics.cpp index 28f8b0a7ae..644abe8b74 100644 --- a/src/aliceVision/sfm/utils/statistics.cpp +++ b/src/aliceVision/sfm/utils/statistics.cpp @@ -17,7 +17,7 @@ double RMSE(const sfmData::SfMData& sfmData) std::vector vec; for (sfmData::Landmarks::const_iterator iterTracks = sfmData.getLandmarks().begin(); iterTracks != sfmData.getLandmarks().end(); ++iterTracks) { - const sfmData::Observations& obs = iterTracks->second.observations; + const sfmData::Observations& obs = iterTracks->second.getObservations(); for (sfmData::Observations::const_iterator itObs = obs.begin(); itObs != obs.end(); ++itObs) { const sfmData::View* view = sfmData.getViews().find(itObs->first)->second.get(); diff --git a/src/aliceVision/sfm/utils/syntheticScene.cpp b/src/aliceVision/sfm/utils/syntheticScene.cpp index e12f00f92e..1f19deb95e 100644 --- a/src/aliceVision/sfm/utils/syntheticScene.cpp +++ b/src/aliceVision/sfm/utils/syntheticScene.cpp @@ -19,9 +19,9 @@ void generateSyntheticMatches(matching::PairwiseMatches& out_pairwiseMatches, co for (const auto& it : sfmData.getLandmarks()) { const sfmData::Landmark& landmark = it.second; - const std::size_t limitMatches = std::min(std::size_t(3), landmark.observations.size()); + const std::size_t limitMatches = std::min(std::size_t(3), landmark.getObservations().size()); - for (auto obsItI = landmark.observations.begin(); obsItI != landmark.observations.end(); ++obsItI) + for (auto obsItI = landmark.getObservations().begin(); obsItI != landmark.getObservations().end(); ++obsItI) { const sfmData::Observation& obsI = obsItI->second; // We don't need matches between all observations. @@ -31,8 +31,8 @@ void generateSyntheticMatches(matching::PairwiseMatches& out_pairwiseMatches, co for (std::size_t j = 1; j < limitMatches; ++j) { ++obsItJ; - if (obsItJ == landmark.observations.end()) - obsItJ = landmark.observations.begin(); + if (obsItJ == landmark.getObservations().end()) + obsItJ = landmark.getObservations().begin(); const sfmData::Observation& obsJ = obsItJ->second; @@ -85,7 +85,7 @@ sfmData::SfMData getInputScene(const NViewDataSet& d, const NViewDatasetConfigur for (int j = 0; j < nviews; ++j) { const Vec2 pt = d._x[j].col(i); - landmark.observations[j] = sfmData::Observation(pt, i, unknownScale); + landmark.getObservations()[j] = sfmData::Observation(pt, i, unknownScale); } sfmData.getLandmarks()[i] = landmark; } @@ -165,7 +165,7 @@ sfmData::SfMData getInputRigScene(const NViewDataSet& d, const NViewDatasetConfi } const Vec2 pt = project(camPinHole->getProjectiveEquivalent(camPose), landmark.X); - landmark.observations[viewId] = sfmData::Observation(pt, landmarkId, unknownScale); + landmark.getObservations()[viewId] = sfmData::Observation(pt, landmarkId, unknownScale); } sfmData.getLandmarks()[landmarkId] = landmark; } diff --git a/src/aliceVision/sfm/utils/syntheticScene.hpp b/src/aliceVision/sfm/utils/syntheticScene.hpp index 7d6430809d..6f99153755 100644 --- a/src/aliceVision/sfm/utils/syntheticScene.hpp +++ b/src/aliceVision/sfm/utils/syntheticScene.hpp @@ -42,7 +42,7 @@ void generateSyntheticFeatures(feature::FeaturesPerView& out_featuresPerView, { const sfmData::Landmark& landmark = it.second; - for (const auto& obsIt : landmark.observations) + for (const auto& obsIt : landmark.getObservations()) { const IndexT viewId = obsIt.first; const sfmData::Observation& obs = obsIt.second; @@ -65,7 +65,7 @@ void generateSyntheticFeatures(feature::FeaturesPerView& out_featuresPerView, { const sfmData::Landmark& landmark = it.second; - for (const auto& obsIt : landmark.observations) + for (const auto& obsIt : landmark.getObservations()) { const IndexT viewId = obsIt.first; const sfmData::Observation& obs = obsIt.second; diff --git a/src/aliceVision/sfmData/Landmark.hpp b/src/aliceVision/sfmData/Landmark.hpp index 119492e8c6..428930a921 100644 --- a/src/aliceVision/sfmData/Landmark.hpp +++ b/src/aliceVision/sfmData/Landmark.hpp @@ -20,34 +20,59 @@ namespace sfmData { /** * @brief Landmark is a 3D point with its 2d observations. */ -struct Landmark +class Landmark { - Landmark() = default; +public: + Landmark() + { + + } + explicit Landmark(feature::EImageDescriberType descType) : descType(descType) - {} + { + + } + Landmark(const Vec3& pos3d, feature::EImageDescriberType descType = feature::EImageDescriberType::UNINITIALIZED, - const Observations& observations = Observations(), const image::RGBColor& color = image::WHITE) : X(pos3d), descType(descType), - observations(observations), rgb(color) - {} + { + } Vec3 X; feature::EImageDescriberType descType = feature::EImageDescriberType::UNINITIALIZED; - Observations observations; image::RGBColor rgb = image::WHITE; //!> the color associated to the point EEstimatorParameterState state = EEstimatorParameterState::REFINED; bool operator==(const Landmark& other) const { - return AreVecNearEqual(X, other.X, 1e-3) && AreVecNearEqual(rgb, other.rgb, 1e-3) && observations == other.observations && - descType == other.descType; + return AreVecNearEqual(X, other.X, 1e-3) && + AreVecNearEqual(rgb, other.rgb, 1e-3) && + _observations == other._observations && + descType == other.descType; + } + + inline bool operator!=(const Landmark& other) const + { + return !(*this == other); + } + + const Observations & getObservations() const + { + return _observations; } - inline bool operator!=(const Landmark& other) const { return !(*this == other); } + + Observations & getObservations() + { + return _observations; + } + +private: + Observations _observations; }; } // namespace sfmData diff --git a/src/aliceVision/sfmData/SfMData.cpp b/src/aliceVision/sfmData/SfMData.cpp index 480ecb8e15..4607324a09 100644 --- a/src/aliceVision/sfmData/SfMData.cpp +++ b/src/aliceVision/sfmData/SfMData.cpp @@ -313,7 +313,7 @@ LandmarksPerView getLandmarksPerViews(const SfMData& sfmData) LandmarksPerView landmarksPerView; for (const auto& landIt : sfmData.getLandmarks()) { - for (const auto& obsIt : landIt.second.observations) + for (const auto& obsIt : landIt.second.getObservations()) { IndexT viewId = obsIt.first; LandmarkIdSet& landmarksSet = landmarksPerView[viewId]; diff --git a/src/aliceVision/sfmData/colorize.cpp b/src/aliceVision/sfmData/colorize.cpp index 80d5c5c8ae..0e8774cdbd 100644 --- a/src/aliceVision/sfmData/colorize.cpp +++ b/src/aliceVision/sfmData/colorize.cpp @@ -49,7 +49,7 @@ void colorizeTracks(SfMData& sfmData) std::map viewsCardinalMap; // for (const auto& landmarkPair : sfmData.getLandmarks()) { - const Observations& observations = landmarkPair.second.observations; + const Observations& observations = landmarkPair.second.getObservations(); for (const auto& observationPair : observations) ++viewsCardinalMap[observationPair.first]; // TODO: 0 } @@ -72,8 +72,8 @@ void colorizeTracks(SfMData& sfmData) for (int i = 0; i < remainingLandmarksToColor.size(); ++i) { Landmark& landmark = remainingLandmarksToColor.at(i); - auto it = landmark.observations.find(viewId); - if (it != landmark.observations.end()) + auto it = landmark.getObservations().find(viewId); + if (it != landmark.getObservations().end()) { viewCardinal.landmarks.push_back(landmark); } @@ -110,7 +110,7 @@ void colorizeTracks(SfMData& sfmData) for (Landmark& landmark : viewCardinal.landmarks) { // color the point - Vec2 pt = landmark.observations.at(view.getViewId()).getCoordinates(); + Vec2 pt = landmark.getObservations().at(view.getViewId()).getCoordinates(); // clamp the pixel position if the feature/marker center is outside the image. pt.x() = clamp(pt.x(), 0.0, static_cast(image.Width() - 1)); pt.y() = clamp(pt.y(), 0.0, static_cast(image.Height() - 1)); diff --git a/src/aliceVision/sfmData/uid.cpp b/src/aliceVision/sfmData/uid.cpp index dce12bb45f..bfb9375dfe 100644 --- a/src/aliceVision/sfmData/uid.cpp +++ b/src/aliceVision/sfmData/uid.cpp @@ -116,7 +116,7 @@ void updateStructureWithNewUID(Landmarks& landmarks, const std::mapsecond.X.data(); std::copy(X, X + 3, std::ostream_iterator(stream, " ")); - const sfmData::Observations& observations = iterLandmarks->second.observations; + const sfmData::Observations& observations = iterLandmarks->second.getObservations(); stream << observations.size() << " "; for (sfmData::Observations::const_iterator iterOb = observations.begin(); iterOb != observations.end(); ++iterOb) { diff --git a/src/aliceVision/sfmDataIO/colmap.cpp b/src/aliceVision/sfmDataIO/colmap.cpp index d283fcd5d5..acc9fb0993 100644 --- a/src/aliceVision/sfmDataIO/colmap.cpp +++ b/src/aliceVision/sfmDataIO/colmap.cpp @@ -247,7 +247,7 @@ PerViewVisibility computePerViewVisibility(const sfmData::SfMData& sfmData, cons for (const auto& land : sfmData.getLandmarks()) { const IndexT landID = land.first; - const auto& observations = land.second.observations; + const auto& observations = land.second.getObservations(); for (const auto& iter : observations) { @@ -362,7 +362,7 @@ void generateColmapPoints3DTxtFile(const sfmData::SfMData& sfmData, const Compat outfile << defaultError; - for (const auto& itObs : iter.second.observations) + for (const auto& itObs : iter.second.getObservations()) { const IndexT viewId = itObs.first; const IndexT featId = itObs.second.getFeatureId(); diff --git a/src/aliceVision/sfmDataIO/compatibilityData/scene_v1.2.6.abc b/src/aliceVision/sfmDataIO/compatibilityData/scene_v1.2.6.abc index 73a4eb5558c4f0a9bb327fe66dd7239c7ee77829..fda94a48d53aa75d98a7b452dc6f69100f386d63 100644 GIT binary patch delta 1518 zcmZwGX;70#7{GB7*iE>GR}mv35{-e-3Nai-1sjzqFbp0AbU-9rqJqdFUR2bWN`Z2S zBlu8{k)v3nvd{44~E=fxf7BDvBnHJ?1Y9e zqdud)oMmw`V}zSr);)6IGg3Mi@g~W26680JStPzy9&3B}vii9)qV4RaT3Lccp}f}i83MQyK1(aGaAR8N7Y1mr}|!gYgJU z$==NGT6b<~Q-H&->aNw+nXs=M`2mP916^9g=a?OBLjriDMFKKfj;14KwY&b%0MEJ6 zSoMo4?TRdh%sqxKPLc;tp-w5IJU*qV1d^{gwe0nHyy{9ios{Les0`=n_c;3vF3XT5 zVeX(X#3Ynn_2M-ro_PToM!f)d9g8;g-;`)Y==4By5O!rWqUztal*h zho5(KxJ(@iGVDC1UKj|KhKY0QHc)EG)$W=4I18!k+Gqdx4ix2*fH#yf&49*P>R`oM zbU&fkX1FycN{BBwL&5k2b!(hbp|V*?$x#~J41@FFwxgaBG9O`1t0c{a0vXxUMza0EdP6$y&`Obn8LIC7aF;_L#mP= z7-neTkx>a<=be1rldf}2a%=G0_{g@8PvnHg@=v?MLVxUHY1R}z*wmSJQw~+uSio*! zw?Ey&&aaNVx|H>r-yWM@ndX2!h;AvRO*7O5v410=FuPiJx-PmfW}Ut362*Fu1XKCm z6NN&>)waU(v{M5vaofU8G z1ExuD!;AD_<#%$P0^j2^ z@@ruaze|ZW-n}?_yQES4DOCF)C-98Nd{VLrdvF=P>f&tofH^-EH9Gi;1=11Ho`S77 zjG{01(L!0)zWI6ab^|>TE8%G>4tDktN)mUZq-_@lB_|5K{DfW$yu5`nzs25ui+#LF J$QHbc`!8<;eP#dv delta 1504 zcmZwGZB)}$9Kdll`u!P$#q}hQYY4((21ty-5HNXAN5v!_JYYvDLV_U^bqH$Wk;Oba zri21rvsi*?rkL7^E+~qgpd*?NPa^3M#*?4|J}f{1q5q%WP2Sz}eet=!b04t585^8S zJXrySLXD865wbNxOO5c2S|RZGsNRKp)U|FXQqXi)`KIJa4UXi)GE8hJL=Ny_m_lWK z+2KatXVs&+lHra=-rGp=La>@69}|Jp6G`>E@7a%hdN~PJO8C}ah_KNBjyT7I;@}xM zSmXI}SirKtFa7{m{Qeq1cv=n|JXHo5u@ZZ+&fiZTV&4zO%t%XqSs zrcCx1zv}0Eeoz?HUHyFD0~X#@22%XQ032(J5nJnT5{Dl(e%k>Ha2I{hgg!_IKpW#8 zocpkN$0{e6b!+y&=$#}B4@3PN`SFi%!w;bpzN)~AD~VP<6g!O_4q%3L#9ey&Ue0L2 z3C}RA(?@_q>{>w%=)Qf?ab8KDdY51<@&1S>n`E~!x_QQd273Ird}QaVmpe3+2AQ(? zJEo<~_>Nd_01@*F4B&sRvU$eB0a~8l zbip-3t@I0v?JoTOptBxtJVMKj1B^@t2OtqpVTox^L*LhXx$Fs5MrzS-BxacL%rmw( z(cVL=_O6jhSk!ihN1`4WxkrHA9nG0HtBw~B`VW_ygiSh zUd#OsRGKZ48qwzS_uQU-T=NPoe|71(`S;xILCNsA9PIx8l12~87Ld2+v@%?*_3huj zz92u&wvJ4Rm@Qt*&5M~Wx_V+P>%DYWQunqjNh0%h;owke0Z!%9FX@a8#i0k^7Rn4B zb;C~elP;yI^A;zhAi;JC=?G_3^Nf$Jq|d-+;2lRzlf-MhL%23&b5}GL$!Juzh(;S{ zfI%~53|e|=Q|ObA9_G52gyrm^(pBWI7zQ=ZxICUlc`9R!p33zN5&} zQHi)x0(`vqGm1m@XHdRu6u9ByiHUAr8z+D47rAd("scale", 0.0)); } - landmark.observations.emplace(obsTree.get("observationId"), observation); + landmark.getObservations().emplace(obsTree.get("observationId"), observation); } } } diff --git a/src/aliceVision/sfmDataIO/sfmDataIO_test.cpp b/src/aliceVision/sfmDataIO/sfmDataIO_test.cpp index c54eb1ee75..0cf4c0757c 100644 --- a/src/aliceVision/sfmDataIO/sfmDataIO_test.cpp +++ b/src/aliceVision/sfmDataIO/sfmDataIO_test.cpp @@ -62,7 +62,7 @@ sfmData::SfMData createTestScene(std::size_t viewsCount = 2, std::size_t observa observations[i] = sfmData::Observation(Vec2(i, i), i, unknownScale); } - sfmData.getLandmarks()[0].observations = observations; + sfmData.getLandmarks()[0].getObservations() = observations; sfmData.getLandmarks()[0].X = Vec3(11, 22, 33); sfmData.getLandmarks()[0].descType = feature::EImageDescriberType::SIFT; diff --git a/src/aliceVision/sfmMvsUtils/visibility.cpp b/src/aliceVision/sfmMvsUtils/visibility.cpp index b595a43905..1428bd9295 100644 --- a/src/aliceVision/sfmMvsUtils/visibility.cpp +++ b/src/aliceVision/sfmMvsUtils/visibility.cpp @@ -28,8 +28,8 @@ void createRefMeshFromDenseSfMData(mesh::Mesh& refMesh, const sfmData::SfMData& const sfmData::Landmark& landmark = landmarkPair.second; mesh::PointVisibility pointVisibility; - pointVisibility.reserve(landmark.observations.size()); - for (const auto& observationPair : landmark.observations) + pointVisibility.reserve(landmark.getObservations().size()); + for (const auto& observationPair : landmark.getObservations()) pointVisibility.push_back(mp.getIndexFromViewId(observationPair.first)); refVisibilities.push_back(pointVisibility); diff --git a/src/software/convert/main_convertSfMFormat.cpp b/src/software/convert/main_convertSfMFormat.cpp index e4268d2984..3c7308f61d 100644 --- a/src/software/convert/main_convertSfMFormat.cpp +++ b/src/software/convert/main_convertSfMFormat.cpp @@ -150,10 +150,10 @@ int aliceVision_main(int argc, char **argv) sfmData::Landmark& landmark = landmarkPair.second; for(const IndexT viewId : viewsToRemove) { - if(landmark.observations.find(viewId) != landmark.observations.end()) - landmark.observations.erase(viewId); + if(landmark.getObservations().find(viewId) != landmark.getObservations().end()) + landmark.getObservations().erase(viewId); } - if(landmark.observations.empty()) + if(landmark.getObservations().empty()) landmarksToRemove.push_back(landmarkPair.first); } diff --git a/src/software/export/main_exportMVE2.cpp b/src/software/export/main_exportMVE2.cpp index bd59af251c..05431e305f 100644 --- a/src/software/export/main_exportMVE2.cpp +++ b/src/software/export/main_exportMVE2.cpp @@ -227,7 +227,7 @@ bool exportToMVE2Format( out << 250 << " " << 100 << " " << 150 << "\n"; // Write arbitrary RGB color, see above note // Tally set of feature observations - const Observations & observations = iterLandmarks->second.observations; + const Observations & observations = iterLandmarks->second.getObservations(); const size_t featureCount = std::distance(observations.begin(), observations.end()); out << featureCount; diff --git a/src/software/export/main_exportMatlab.cpp b/src/software/export/main_exportMatlab.cpp index bb41b0488d..9eeecb0009 100644 --- a/src/software/export/main_exportMatlab.cpp +++ b/src/software/export/main_exportMatlab.cpp @@ -53,7 +53,7 @@ bool exportToMatlab( const IndexT landmarkId = s.first; const Landmark& landmark = s.second; landmarksFile << landmarkId << " " << landmark.X[0] << " " << landmark.X[1] << " " << landmark.X[2] << "\n"; - for(const auto& obs: landmark.observations) + for(const auto& obs: landmark.getObservations()) { const IndexT obsView = obs.first; // The ID of the view that provides this 2D observation. observationsPerView[obsView].push_back(Observation(obs.second.getCoordinates(), landmarkId, unknownScale)); diff --git a/src/software/export/main_exportPMVS.cpp b/src/software/export/main_exportPMVS.cpp index ad5b04ac56..cfd6ffbf21 100644 --- a/src/software/export/main_exportPMVS.cpp +++ b/src/software/export/main_exportPMVS.cpp @@ -173,7 +173,7 @@ bool exportToPMVSFormat( itL != sfm_data.getLandmarks().end(); ++itL) { const Landmark & landmark = itL->second; - const Observations & observations = landmark.observations; + const Observations & observations = landmark.getObservations(); for (Observations::const_iterator itOb = observations.begin(); itOb != observations.end(); ++itOb) { @@ -304,7 +304,7 @@ bool exportToBundlerFormat( iter != sfm_data.getLandmarks().end(); ++iter) { const Landmark & landmark = iter->second; - const Observations & observations = landmark.observations; + const Observations & observations = landmark.getObservations(); const Vec3 & X = landmark.X; // X, color, obsCount os << X[0] << " " << X[1] << " " << X[2] << os.widen('\n') diff --git a/src/software/pipeline/main_checkerboardCalibration.cpp b/src/software/pipeline/main_checkerboardCalibration.cpp index 413458c65b..00f8626141 100644 --- a/src/software/pipeline/main_checkerboardCalibration.cpp +++ b/src/software/pipeline/main_checkerboardCalibration.cpp @@ -338,7 +338,7 @@ bool estimateIntrinsicsPoses(sfmData::SfMData& sfmData, // Add observation sfmData::Observation obs(p, idxlandmark, 1.0); - sfmData.getLandmarks()[idxlandmark].observations[viewId] = obs; + sfmData.getLandmarks()[idxlandmark].getObservations()[viewId] = obs; } } } diff --git a/src/software/pipeline/main_meshing.cpp b/src/software/pipeline/main_meshing.cpp index efe74958c9..da68ecc9f2 100644 --- a/src/software/pipeline/main_meshing.cpp +++ b/src/software/pipeline/main_meshing.cpp @@ -110,7 +110,7 @@ void createDenseSfMData(const sfmData::SfMData& sfmData, const sfmData::View& view = sfmData.getView(mp.getViewId(cam)); const camera::IntrinsicBase* intrinsicPtr = sfmData.getIntrinsicPtr(view.getIntrinsicId()); const sfmData::Observation observation(intrinsicPtr->project(sfmData.getPose(view).getTransform(), pt3D.homogeneous(), true), UndefinedIndexT, unknownScale); // apply distortion - landmark.observations[view.getViewId()] = observation; + landmark.getObservations()[view.getViewId()] = observation; } } outSfmData.getLandmarks()[i] = landmark; @@ -123,7 +123,7 @@ void removeLandmarksWithoutObservations(sfmData::SfMData& sfmData) auto& landmarks = sfmData.getLandmarks(); for(auto it = landmarks.begin(); it != landmarks.end();) { - if(it->second.observations.empty()) + if(it->second.getObservations().empty()) it = landmarks.erase(it); else ++it; diff --git a/src/software/pipeline/main_nodalSfM.cpp b/src/software/pipeline/main_nodalSfM.cpp index 63dd436eba..ca64845759 100644 --- a/src/software/pipeline/main_nodalSfM.cpp +++ b/src/software/pipeline/main_nodalSfM.cpp @@ -179,8 +179,8 @@ void buildInitialWorld(sfmData::SfMData& sfmData, const feature::FeaturesPerView sfmData::Landmark l(track.descType); l.X = refP; - l.observations[pair.reference] = sfmData::Observation(refV, refFeatureId, refFeatures[refFeatureId].scale()); - l.observations[pair.next] = sfmData::Observation(nextV, nextFeatureId, nextFeatures[nextFeatureId].scale()); + l.getObservations()[pair.reference] = sfmData::Observation(refV, refFeatureId, refFeatures[refFeatureId].scale()); + l.getObservations()[pair.next] = sfmData::Observation(nextV, nextFeatureId, nextFeatures[nextFeatureId].scale()); landmarks[id] = l; } @@ -281,7 +281,7 @@ bool localizeNext(sfmData::SfMData& sfmData, const feature::FeaturesPerView & fe const feature::PointFeatures& newViewFeatures = newViewFeaturesPerDesc.at(track.descType); IndexT newViewFeatureId = track.featPerView.at(newViewId).featureId; auto & feat = newViewFeatures[newViewFeatureId]; - landmarks[trackId].observations[newViewId] = sfmData::Observation(feat.coords().cast(), newViewFeatureId, feat.scale()); + landmarks[trackId].getObservations()[newViewId] = sfmData::Observation(feat.coords().cast(), newViewFeatureId, feat.scale()); } return true; @@ -355,8 +355,8 @@ bool addPoints(sfmData::SfMData& sfmData, const feature::FeaturesPerView & featu sfmData::Landmark l(track.descType); l.X = world_R_new * newP; - l.observations[newViewId] = sfmData::Observation(newV, newViewFeatureId, refViewFeatures[refViewFeatureId].scale()); - l.observations[pV.first] = sfmData::Observation(refV, refViewFeatureId, newViewFeatures[newViewFeatureId].scale()); + l.getObservations()[newViewId] = sfmData::Observation(newV, newViewFeatureId, refViewFeatures[refViewFeatureId].scale()); + l.getObservations()[pV.first] = sfmData::Observation(refV, refViewFeatureId, newViewFeatures[newViewFeatureId].scale()); landmarks[trackId] = l; } diff --git a/src/software/pipeline/main_panoramaEstimation.cpp b/src/software/pipeline/main_panoramaEstimation.cpp index 91d58fe53f..8cca66e611 100644 --- a/src/software/pipeline/main_panoramaEstimation.cpp +++ b/src/software/pipeline/main_panoramaEstimation.cpp @@ -310,7 +310,7 @@ int aliceVision_main(int argc, char** argv) std::set viewsWithObservations; for(const auto& landmarkIt : outSfmData.getLandmarks()) { - for(const auto& obsIt : landmarkIt.second.observations) + for(const auto& obsIt : landmarkIt.second.getObservations()) { viewsWithObservations.insert(obsIt.first); } diff --git a/src/software/pipeline/main_sfmBootstraping.cpp b/src/software/pipeline/main_sfmBootstraping.cpp index 92555a52cc..79e41b104c 100644 --- a/src/software/pipeline/main_sfmBootstraping.cpp +++ b/src/software/pipeline/main_sfmBootstraping.cpp @@ -277,8 +277,8 @@ bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pai nextObs.setScale(nextFeatures[nextFeatureId].scale()); nextObs.setCoordinates(nextpt); - landmark.observations[pair.reference] = refObs; - landmark.observations[pair.next] = nextObs; + landmark.getObservations()[pair.reference] = refObs; + landmark.getObservations()[pair.next] = nextObs; sfmData.getLandmarks()[trackId] = landmark; } diff --git a/src/software/utils/main_frustumFiltering.cpp b/src/software/utils/main_frustumFiltering.cpp index b0a3d76c57..59f5df0a53 100644 --- a/src/software/utils/main_frustumFiltering.cpp +++ b/src/software/utils/main_frustumFiltering.cpp @@ -39,12 +39,12 @@ PairSet BuildPairsFromStructureObservations(const sfmData::SfMData& sfmData) itL != sfmData.getLandmarks().end(); ++itL) { const sfmData::Landmark & landmark = itL->second; - for(const auto& iterI : landmark.observations) + for(const auto& iterI : landmark.getObservations()) { const IndexT id_viewI = iterI.first; - sfmData::Observations::const_iterator iterJ = landmark.observations.begin(); + sfmData::Observations::const_iterator iterJ = landmark.getObservations().begin(); std::advance(iterJ, 1); - for (; iterJ != landmark.observations.end(); ++iterJ) + for (; iterJ != landmark.getObservations().end(); ++iterJ) { const IndexT id_viewJ = iterJ->first; pairs.insert( std::make_pair(id_viewI,id_viewJ)); diff --git a/src/software/utils/main_sfmRegression.cpp b/src/software/utils/main_sfmRegression.cpp index 9943270645..642a664434 100644 --- a/src/software/utils/main_sfmRegression.cpp +++ b/src/software/utils/main_sfmRegression.cpp @@ -113,8 +113,8 @@ void generateSampleSceneOnePlane(sfmData::SfMData & returnSfmDataGT, sfmData::Sf continue; } - pl.second.observations[pp.first] = obs; - lEst.observations[pp.first] = obs; + pl.second.getObservations()[pp.first] = obs; + lEst.getObservations()[pp.first] = obs; } } diff --git a/src/software/utils/main_sfmTransfer.cpp b/src/software/utils/main_sfmTransfer.cpp index 2a3fc79c9f..21a6f4045e 100644 --- a/src/software/utils/main_sfmTransfer.cpp +++ b/src/software/utils/main_sfmTransfer.cpp @@ -301,21 +301,21 @@ int aliceVision_main(int argc, char **argv) aliceVision::sfmData::Landmark newLandmark = landIt.second; // Clear all observations : - newLandmark.observations.clear(); + newLandmark.getObservations().clear(); // For all observations of the ref landmark : - for (const auto& obsIt : landIt.second.observations) + for (const auto& obsIt : landIt.second.getObservations()) { const IndexT viewId = obsIt.first; // If the observation view has a correspondance in the other sfmData, we copy it : if (commonViewsMap.find(viewId) != commonViewsMap.end() ) { - newLandmark.observations.emplace(commonViewsMap.at(viewId), landIt.second.observations.at(viewId)); + newLandmark.getObservations().emplace(commonViewsMap.at(viewId), landIt.second.getObservations().at(viewId)); } } // If the landmark has at least one observation in the new scene, we copy it : - if(newLandmark.observations.size() > 0) + if(newLandmark.getObservations().size() > 0) { newLandmarks.emplace(landIt.first,newLandmark); } From 28b86885f4aa3ec33a2589b1428f3654503d020f Mon Sep 17 00:00:00 2001 From: Fabien Servant Date: Thu, 14 Dec 2023 09:43:30 +0100 Subject: [PATCH 3/3] rename _structure to _landmarks for homogeneity --- src/aliceVision/sfmData/SfMData.cpp | 14 +++++++------- src/aliceVision/sfmData/SfMData.hpp | 6 +++--- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/aliceVision/sfmData/SfMData.cpp b/src/aliceVision/sfmData/SfMData.cpp index 4607324a09..ad0234f6b5 100644 --- a/src/aliceVision/sfmData/SfMData.cpp +++ b/src/aliceVision/sfmData/SfMData.cpp @@ -79,12 +79,12 @@ bool SfMData::operator==(const SfMData& other) const } // Points IDs are not preserved - if (_structure.size() != other._structure.size()) + if (_landmarks.size() != other._landmarks.size()) return false; - Landmarks::const_iterator landMarkIt = _structure.begin(); - Landmarks::const_iterator otherLandmarkIt = other._structure.begin(); - for (; landMarkIt != _structure.end() && otherLandmarkIt != other._structure.end(); ++landMarkIt, ++otherLandmarkIt) + Landmarks::const_iterator landMarkIt = _landmarks.begin(); + Landmarks::const_iterator otherLandmarkIt = other._landmarks.begin(); + for (; landMarkIt != _landmarks.end() && otherLandmarkIt != other._landmarks.end(); ++landMarkIt, ++otherLandmarkIt) { // Points IDs are not preserved // Landmark @@ -267,7 +267,7 @@ void SfMData::combine(const SfMData& sfmData) _rigs.insert(sfmData._rigs.begin(), sfmData._rigs.end()); // structure - _structure.insert(sfmData._structure.begin(), sfmData._structure.end()); + _landmarks.insert(sfmData._landmarks.begin(), sfmData._landmarks.end()); // constraints constraints2d.insert(constraints2d.end(), sfmData.constraints2d.begin(), sfmData.constraints2d.end()); @@ -277,7 +277,7 @@ void SfMData::clear() { _views.clear(); _intrinsics.clear(); - _structure.clear(); + _landmarks.clear(); _posesUncertainty.clear(); _landmarksUncertainty.clear(); constraints2d.clear(); @@ -297,7 +297,7 @@ void SfMData::resetParameterStates() pp.second.initializeState(); } - for (auto& pl : _structure) + for (auto& pl : _landmarks) { pl.second.state = EEstimatorParameterState::REFINED; } diff --git a/src/aliceVision/sfmData/SfMData.hpp b/src/aliceVision/sfmData/SfMData.hpp index 14b7696016..10700968d2 100644 --- a/src/aliceVision/sfmData/SfMData.hpp +++ b/src/aliceVision/sfmData/SfMData.hpp @@ -117,8 +117,8 @@ class SfMData * @brief Get landmarks * @return landmarks */ - const Landmarks& getLandmarks() const { return _structure; } - Landmarks& getLandmarks() { return _structure; } + const Landmarks& getLandmarks() const { return _landmarks; } + Landmarks& getLandmarks() { return _landmarks; } /** * @brief Get Constraints2D @@ -521,7 +521,7 @@ class SfMData private: /// Structure (3D points with their 2D observations) - Landmarks _structure; + Landmarks _landmarks; /// Considered camera intrinsics (indexed by view.getIntrinsicId()) Intrinsics _intrinsics; /// Considered views