diff --git a/meshroom/aliceVision/SfmExpanding.py b/meshroom/aliceVision/SfmExpanding.py index cf569a9382..de015fb2d6 100644 --- a/meshroom/aliceVision/SfmExpanding.py +++ b/meshroom/aliceVision/SfmExpanding.py @@ -77,6 +77,101 @@ class SfMExpanding(desc.AVCommandLineNode): "by avoiding computation of the Bundle Adjustment on areas that are not changing.", value=True, ), + desc.BoolParam( + name="useTemporalConstraint", + label="Temporal Constraint", + description="Adds a temporal smoothness constraint to the bundle adjustment.", + value=False, + ), + desc.FloatParam( + name="tscPositionWeight", + label="Temporal Constraint Position Weight", + description="Controls the weight of the temporal constraint applied to camera positions. Higher values enforce smoother camera path.", + value=10.0, + range=(0.0, 100.0, 0.1), + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), + desc.FloatParam( + name="tscOrientationWeight", + label="Temporal Constraint Orientation Weight", + description="Controls the weight of the temporal constraint applied to camera orientations. Higher values enforce smoother camera rotation.", + value=10.0, + range=(0.0, 100.0, 0.1), + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), + desc.FloatParam( + name="tscRegularizationWeight", + label="Temporal Constraint Regularization Weight", + description="Controls the strength of regularization applied to the temporal constraint.", + value=0.0, + range=(0.0, 100.0, 0.1), + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), + desc.FloatParam( + name="tscC0positionWeight", + label="Temporal Constraint C0 Position Weight", + description="Controls the weight of the continuity constraint on camera positions in the temporal constraint. Higher values enforce smoother transitions in position, reducing abrupt changes of position.", + value=0.0, + range=(0.0, 1.0, 0.01), + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), + desc.FloatParam( + name="tscC1positionWeight", + label="Temporal Constraint C1 Position Weight", + description="Controls the weight of the first derivative of camera position in the temporal constraint. Higher values enforce continuity of the camera velocity, reducing abrupt changes of velocity.", + value=1.0, + range=(0.0, 1.0, 0.01), + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), + desc.FloatParam( + name="tscC2positionWeight", + label="Temporal Constraint C2 Position Weight", + description="Controls the weight of the second derivative of camera position in the temporal constraint. Higher values enforce continuity of the camera acceleration, reducing abrupt changes of acceleration.", + value=1.0, + range=(0.0, 1.0, 0.01), + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), + desc.FloatParam( + name="tscC0orientationWeight", + label="Temporal Constraint C0 Orientation Weight", + description="Controls the weight of the continuity constraint on camera orientation in the temporal constraint. Higher values enforce smoother transitions, reducing abrupt changes of the camera orientation.", + value=0.0, + range=(0.0, 1.0, 0.01), + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), + desc.FloatParam( + name="tscC1orientationWeight", + label="Temporal Constraint C1 Orientation Weight", + description="Controls the weight of the first derivative of camera orientation in the temporal constraint. Higher values enforce continuity of the rotation velocity, reducing abrupt changes of rotation velocity.", + value=1.0, + range=(0.0, 1.0, 0.01), + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), + desc.FloatParam( + name="tscC2orientationWeight", + label="Temporal Constraint C2 Orientation Weight", + description="Controls the weight of the second derivative of camera orientation in the temporal constraint. Higher values enforce continuity of the rotation acceleration, reducing abrupt changes of rotation acceleration.", + value=1.0, + range=(0.0, 1.0, 0.01), + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), + desc.BoolParam( + name="exitAfterPoseInterpolation", + label="Only Pose Interpolation", + description="Exit after pose interpolation, before bundle adjustment. (for debug)", + value=False, + advanced=True, + enabled=lambda node: node.useTemporalConstraint.value, + ), desc.IntParam( name="localBAGraphDistance", label="LocalBA Graph Distance", diff --git a/meshroom/aliceVision/TracksSimulating.py b/meshroom/aliceVision/TracksSimulating.py index 2fe718fb70..3789888a34 100644 --- a/meshroom/aliceVision/TracksSimulating.py +++ b/meshroom/aliceVision/TracksSimulating.py @@ -43,6 +43,14 @@ class TracksSimulating(desc.AVCommandLineNode): invalidate=True, advanced=True, ), + desc.BoolParam( + name="randomNoiseVariancePerView", + label="Random Variance Per View", + description="Use different noise variance per view.", + value=False, + invalidate=True, + advanced=True, + ), desc.ChoiceParam( name="verboseLevel", label="Verbose Level", diff --git a/src/aliceVision/sfm/CMakeLists.txt b/src/aliceVision/sfm/CMakeLists.txt index 0d13601cb6..9444843169 100644 --- a/src/aliceVision/sfm/CMakeLists.txt +++ b/src/aliceVision/sfm/CMakeLists.txt @@ -2,11 +2,13 @@ set(sfm_bundle_files_headers bundle/BundleAdjustment.hpp bundle/BundleAdjustmentCeres.hpp + utils/poseFilter.hpp ) # Sources set(sfm_bundle_files_sources bundle/BundleAdjustmentCeres.cpp + utils/poseFilter.cpp ) set(sfm_files_headers diff --git a/src/aliceVision/sfm/bundle/BundleAdjustment.hpp b/src/aliceVision/sfm/bundle/BundleAdjustment.hpp index cbedbcb8e6..79ceb812bd 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustment.hpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustment.hpp @@ -75,6 +75,19 @@ inline std::istream& operator>>(std::istream& in, EFeatureConstraint& m) return in; } +struct TemporalConstraintParams +{ + double positionWeight = 10.0; + double orientationWeight = 10.0; + double regularizationWeight = 0.0; + double c0positionWeight = 0.0; + double c1positionWeight = 1.0; + double c2positionWeight = 1.0; + double c0orientationWeight = 0.0; + double c1orientationWeight = 1.0; + double c2orientationWeight = 1.0; +}; + class BundleAdjustment { public: @@ -102,6 +115,7 @@ class BundleAdjustment REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA = 32, //< refine the optical offset only if we have a minimum number of cameras REFINE_INTRINSICS_DISTORTION = 64, //< refine the distortion parameters REFINE_STRUCTURE_AS_NORMALS = 128, //< Structure lies on a sphere (Pure rotation) + REFINE_TEMPORAL_SMOOTHNESS_CONSTRAINT = 256, //< add a temporal constraint to smooth camera positions/orientation /// Refine all intrinsics parameters REFINE_INTRINSICS_ALL = REFINE_INTRINSICS_FOCAL | REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA | REFINE_INTRINSICS_DISTORTION, /// Refine all parameters diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index dc7a37456d..6f8d8cf97b 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -13,8 +13,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -109,14 +111,14 @@ bool BundleAdjustmentCeres::Statistics::exportToFile(const std::string& folder, posesWithDistUpperThanTen += it.second; os << time << ";" << states[EParameter::POSE][EEstimatorParameterState::REFINED] << ";" - << states[EParameter::POSE][EEstimatorParameterState::CONSTANT] << ";" + << states[EParameter::POSE][EEstimatorParameterState::CONSTANT] << ";" << states[EParameter::POSE][EEstimatorParameterState::IGNORED] << ";" - << states[EParameter::LANDMARK][EEstimatorParameterState::REFINED] << ";" - << states[EParameter::LANDMARK][EEstimatorParameterState::CONSTANT] << ";" + << states[EParameter::LANDMARK][EEstimatorParameterState::REFINED] << ";" + << states[EParameter::LANDMARK][EEstimatorParameterState::CONSTANT] << ";" << states[EParameter::LANDMARK][EEstimatorParameterState::IGNORED] << ";" - << states[EParameter::INTRINSIC][EEstimatorParameterState::REFINED] << ";" - << states[EParameter::INTRINSIC][EEstimatorParameterState::CONSTANT] << ";" - << states[EParameter::INTRINSIC][EEstimatorParameterState::IGNORED] << ";" + << states[EParameter::INTRINSIC][EEstimatorParameterState::REFINED] << ";" + << states[EParameter::INTRINSIC][EEstimatorParameterState::CONSTANT] << ";" + << states[EParameter::INTRINSIC][EEstimatorParameterState::IGNORED] << ";" << nbResidualBlocks << ";" << nbSuccessfullIterations << ";" << nbUnsuccessfullIterations << ";" << RMSEinitial << ";" << RMSEfinal << ";"; @@ -322,7 +324,7 @@ void BundleAdjustmentCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmDa ceres::Problem& problem) { const bool refineIntrinsicsOpticalCenter = - (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) + (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) || (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA); const bool refineIntrinsicsFocalLength = refineOptions & REFINE_INTRINSICS_FOCAL; const bool refineIntrinsicsDistortion = refineOptions & REFINE_INTRINSICS_DISTORTION; @@ -397,8 +399,8 @@ void BundleAdjustmentCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmDa _statistics.addState(EParameter::INTRINSIC, EEstimatorParameterState::CONSTANT); problem.SetParameterBlockConstant(intrinsicBlockPtr); } - else - { + else + { // constant parameters bool lockCenter = false; bool lockFocal = false; @@ -410,8 +412,8 @@ void BundleAdjustmentCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmDa // refine the focal length if (!lockFocal) { - if (intrinsicScaleOffset->getInitialScale().x() > 0 - && intrinsicScaleOffset->getInitialScale().y() > 0 + if (intrinsicScaleOffset->getInitialScale().x() > 0 + && intrinsicScaleOffset->getInitialScale().y() > 0 && _ceresOptions.useFocalPrior) { const double maxFocalError = 0.2 * std::max(intrinsicPtr->w(), intrinsicPtr->h()); @@ -443,11 +445,11 @@ void BundleAdjustmentCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmDa // optical center lockCenter = intrinsicScaleOffset->isOffsetLocked(); - - bool validRefineCenter = (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) + + bool validRefineCenter = (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS) || ( - (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA) - && _minNbImagesToRefineOpticalCenter > 0 + (refineOptions & REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA) + && _minNbImagesToRefineOpticalCenter > 0 && usageCount >= _minNbImagesToRefineOpticalCenter ); @@ -470,12 +472,12 @@ void BundleAdjustmentCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmDa problem.SetParameterLowerBound(intrinsicBlockPtr, 3, opticalCenterMinPercent * intrinsicPtr->h()); problem.SetParameterUpperBound(intrinsicBlockPtr, 3, opticalCenterMaxPercent * intrinsicPtr->h()); } - + auto * subsetManifold = new IntrinsicsManifold( - intrinsicBlock.size(), - focalRatio, - lockFocal, - lockRatio, + intrinsicBlock.size(), + focalRatio, + lockFocal, + lockRatio, lockCenter ); @@ -495,11 +497,11 @@ void BundleAdjustmentCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmDa distortionBlock = distortion->getParameters(); double* distortionBlockPtr = distortionBlock.data(); problem.AddParameterBlock(distortionBlockPtr, distortionBlock.size()); - - if (!refineIntrinsicsDistortion + + if (!refineIntrinsicsDistortion || isod->getDistortionInitializationMode() == camera::EInitMode::CALIBRATED || distortion->isLocked() - || !refineIntrinsics + || !refineIntrinsics || intrinsicPtr->getState() == EEstimatorParameterState::CONSTANT ) { @@ -508,12 +510,11 @@ void BundleAdjustmentCeres::addIntrinsicsToProblem(const sfmData::SfMData& sfmDa } } - _statistics.addState(EParameter::INTRINSIC, EEstimatorParameterState::REFINED); } } -void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) +void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem, std::vector& blockIds) { const bool refineStructure = refineOptions & REFINE_STRUCTURE; @@ -613,7 +614,8 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat params.push_back(rigBlockPtr); params.push_back(landmarkBlockPtr); - problem.AddResidualBlock(costFunction, lossFunction, params); + ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, lossFunction, params); + blockIds.push_back(blockId); } else if (referencePoseBlockPtr != nullptr) { @@ -635,26 +637,28 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat else { ceres::CostFunction* costFunction = ProjectionSimpleErrorFunctor::createCostFunction(intrinsic, observation); - + std::vector params; params.push_back(intrinsicBlockPtr); params.push_back(distortionBlockPtr); params.push_back(poseBlockPtr); params.push_back(landmarkBlockPtr); - problem.AddResidualBlock(costFunction, lossFunction, params); + ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, lossFunction, params); + blockIds.push_back(blockId); } if (observation.getDepth() > 0.0) { const double depthVariance = 0.1; //10 cm ? ceres::CostFunction* costFunction = DepthErrorFunctor::createCostFunction(observation, depthVariance); - + std::vector params; params.push_back(poseBlockPtr); params.push_back(landmarkBlockPtr); - problem.AddResidualBlock(costFunction, nullptr, params); + ceres::ResidualBlockId blockId = problem.AddResidualBlock(costFunction, nullptr, params); + blockIds.push_back(blockId); } if (!refineStructure || landmark.getState() == EEstimatorParameterState::CONSTANT || landmark.isPrecise()) @@ -673,7 +677,7 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat void BundleAdjustmentCeres::addSurveyPointsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) { - + // build the residual blocks corresponding to the track observations for (const auto& [idView, vspoints] : sfmData.getSurveyPoints()) { @@ -710,9 +714,9 @@ void BundleAdjustmentCeres::addSurveyPointsToProblem(const sfmData::SfMData& sfm for (const auto & spoint: vspoints) { sfmData::Observation observation(spoint.survey, 0, 1.0); - ceres::CostFunction* costFunction = + ceres::CostFunction* costFunction = ProjectionSurveyErrorFunctor::createCostFunction(intrinsic, spoint.point3d, observation); - + std::vector params; params.push_back(intrinsicBlockPtr); params.push_back(distortionBlockPtr); @@ -729,7 +733,7 @@ void BundleAdjustmentCeres::addConstraints2DToProblem(const sfmData::SfMData& sf // note: set it to NULL if you don't want use a lossFunction. ceres::LossFunction* lossFunction = _ceresOptions.lossFunction.get(); double* fakeDistortionBlockPtr = &_fakeDistortionBlock; - + for (const auto& constraint : sfmData.getConstraints2D()) { const sfmData::View& view_1 = sfmData.getView(constraint.ViewFirst); @@ -764,7 +768,7 @@ void BundleAdjustmentCeres::addConstraints2DToProblem(const sfmData::SfMData& sf ceres::CostFunction* costFunction = Constraint2dErrorFunctor::createCostFunction(intrinsicObject1, constraint.ObservationFirst, constraint.ObservationSecond); - + problem.AddResidualBlock(costFunction, lossFunction, intrinsicBlockPtr_1, distortionBlockPtr_1, poseBlockPtr_1, poseBlockPtr_2); } } @@ -791,7 +795,7 @@ void BundleAdjustmentCeres::addConstraintsPointToProblem(const sfmData::SfMData& double * ldata = _landmarksBlocks.at(landmarkId).data(); ceres::CostFunction* costFunction = ConstraintPointErrorFunctor::createCostFunction(l, constraint.normal); - + problem.AddResidualBlock(costFunction, lossFunction, ldata); } } @@ -819,7 +823,118 @@ void BundleAdjustmentCeres::addRotationPriorsToProblem(const sfmData::SfMData& s } } -void BundleAdjustmentCeres::createProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem) +void BundleAdjustmentCeres::addTemporalSmoothnessToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem, std::vector& blockIds) +{ + const bool temporalSmoothness = refineOptions & BundleAdjustment::REFINE_TEMPORAL_SMOOTHNESS_CONSTRAINT; + + if (!temporalSmoothness) + { + return; + } + + // set a LossFunction to be less penalized by false measurements. + // note: set it to NULL if you don't want use a lossFunction. + ceres::LossFunction* lossFunction = nullptr; + + double focalScale = meanFocalLength(sfmData); + + TemporalConstraintParams tempConstrParams = _ceresOptions.temporalConstraintParams; + + double positionWeight = focalScale * tempConstrParams.positionWeight; + double orientationWeight = focalScale * tempConstrParams.orientationWeight; + + const double c0pWeight = tempConstrParams.c0positionWeight; + const double c1pWeight = tempConstrParams.c1positionWeight; + const double c2pWeight = tempConstrParams.c2positionWeight; + const double c0oWeight = tempConstrParams.c0orientationWeight; + const double c1oWeight = tempConstrParams.c1orientationWeight; + const double c2oWeight = tempConstrParams.c2orientationWeight; + + double regularizationWeight = tempConstrParams.regularizationWeight; + regularizationWeight = regularizationWeight * positionWeight * std::max({c0pWeight, c1pWeight, c2pWeight, c0oWeight, c1oWeight, c2oWeight}); + + std::vector poseIdsVec; + IndexT firstViewWithPose; + IndexT lastViewWithPose; + if (!getOrderedPoseIds(sfmData, poseIdsVec, firstViewWithPose, lastViewWithPose)) + { + ALICEVISION_THROW_ERROR("Unable to get ordered frameIDs."); + } + + ALICEVISION_LOG_INFO("First view with pose : " << firstViewWithPose); + ALICEVISION_LOG_INFO("Last view with pose : " << lastViewWithPose); + + std::vector poseBlockPtrs; + + for (int frameIdx = firstViewWithPose; frameIdx <= lastViewWithPose; frameIdx++) + { + poseBlockPtrs.push_back(_posesBlocks.at(poseIdsVec.at(frameIdx)).data()); + _linearSolverOrdering.AddElementToGroup(poseBlockPtrs[frameIdx-firstViewWithPose], 1); + } + + ceres::ResidualBlockId blockId; + + for (int frameIdx = firstViewWithPose + 3; frameIdx <= lastViewWithPose; frameIdx++) + { + std::vector posesParams(poseBlockPtrs.begin()+frameIdx-firstViewWithPose-3, + poseBlockPtrs.begin()+frameIdx-firstViewWithPose+1); + + // Add constraint for the first views + if (frameIdx == firstViewWithPose+3) + { + ceres::CostFunction* costFunction1 = TemporalConstraintFunctor::createCostFunction( + positionWeight, orientationWeight, c0pWeight, c1pWeight, c2pWeight, c0oWeight, c1oWeight, c2oWeight, 1); + blockId = problem.AddResidualBlock(costFunction1, lossFunction, posesParams); + blockIds.push_back(blockId); + + ceres::CostFunction* costFunction2 = TemporalConstraintFunctor::createCostFunction( + positionWeight, orientationWeight, c0pWeight, c1pWeight, c2pWeight, c0oWeight, c1oWeight, c2oWeight, 2); + blockId = problem.AddResidualBlock(costFunction2, lossFunction, posesParams); + blockIds.push_back(blockId); + } + + ceres::CostFunction* costFunction = TemporalConstraintFunctor::createCostFunction( + positionWeight, orientationWeight, c0pWeight, c1pWeight, c2pWeight, c0oWeight, c1oWeight, c2oWeight, 0); + blockId = problem.AddResidualBlock(costFunction, lossFunction, posesParams); + blockIds.push_back(blockId); + } + + if (regularizationWeight != 0.0) + { + bool trackLengthReg = false; + bool landmarks2viewsReg = true; + + if (trackLengthReg) + { + double trackLength = cameraTrackSquareLength(poseBlockPtrs); + ceres::CostFunction* regFunction = TemporalRegularizationFunctor::createCostFunction + (regularizationWeight, trackLength, poseBlockPtrs.size()); + ceres::LossFunction* reg_loss_function = nullptr; + problem.AddResidualBlock(regFunction, reg_loss_function, poseBlockPtrs); + } + + if (landmarks2viewsReg) + { + std::vector landmarks; + + for (auto& [landmarkID, landmarkPtr] : _landmarksBlocks) + landmarks.push_back(landmarkPtr.data()); + + std::vector meanLandmarks2viewsVector(3); + + meanLandmarks2viewsPose(landmarks, poseBlockPtrs, meanLandmarks2viewsVector); + + ceres::CostFunction* regFunction = Landmarks2viewsRegularizationFunctor::createCostFunction + (regularizationWeight, landmarks, meanLandmarks2viewsVector, poseBlockPtrs.size()); + ceres::LossFunction* reg_loss_function = nullptr; + problem.AddResidualBlock(regFunction, reg_loss_function, poseBlockPtrs); + } + } + + ALICEVISION_LOG_INFO("SfmBundle::Temporal Smoothness added to problem"); +} + +void BundleAdjustmentCeres::createProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem, std::vector& landmarksBlockIds, std::vector& temporalConstraintBlockIds) { // clear previously computed data resetProblem(); @@ -835,7 +950,7 @@ void BundleAdjustmentCeres::createProblem(const sfmData::SfMData& sfmData, ERefi addIntrinsicsToProblem(sfmData, refineOptions, problem); // add SfM landmarks to the Ceres problem - addLandmarksToProblem(sfmData, refineOptions, problem); + addLandmarksToProblem(sfmData, refineOptions, problem, landmarksBlockIds); // add SfM landmarks to the Ceres problem addSurveyPointsToProblem(sfmData, refineOptions, problem); @@ -848,6 +963,9 @@ void BundleAdjustmentCeres::createProblem(const sfmData::SfMData& sfmData, ERefi // add rotation priors to the Ceres problem addRotationPriorsToProblem(sfmData, refineOptions, problem); + + // add temporal smoothness constraint to the Ceres problem + addTemporalSmoothnessToProblem(sfmData, refineOptions, problem, temporalConstraintBlockIds); } void BundleAdjustmentCeres::resetProblem() @@ -960,11 +1078,14 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin void BundleAdjustmentCeres::createJacobian(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::CRSMatrix& jacobian) { + std::vector landmarksBlockIds; + std::vector temporalConstraintBlockIds; + // create problem ceres::Problem::Options problemOptions; problemOptions.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; ceres::Problem problem(problemOptions); - createProblem(sfmData, refineOptions, problem); + createProblem(sfmData, refineOptions, problem, landmarksBlockIds, temporalConstraintBlockIds); // configure Jacobian engine double cost = 0.0; @@ -981,12 +1102,29 @@ bool BundleAdjustmentCeres::adjust(sfmData::SfMData& sfmData, ERefineOptions ref { ALICEVISION_LOG_INFO("BundleAdjustmentCeres::adjust start"); + std::vector landmarksBlockIds; + std::vector temporalConstraintBlockIds; + // create problem ceres::Problem::Options problemOptions; problemOptions.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problemOptions.evaluation_callback = this; ceres::Problem problem(problemOptions); - createProblem(sfmData, refineOptions, problem); + createProblem(sfmData, refineOptions, problem, landmarksBlockIds, temporalConstraintBlockIds); + + ALICEVISION_LOG_INFO("landmarksBlockIds : " << landmarksBlockIds.size()); + ceres::Problem::EvaluateOptions evaluateOptions; + evaluateOptions.residual_blocks = landmarksBlockIds; + double cost; + problem.Evaluate(evaluateOptions, &cost, NULL, NULL, NULL); + ALICEVISION_LOG_INFO("landmarksBlocks cost : " << cost); + + if (temporalConstraintBlockIds.size() != 0) + { + evaluateOptions.residual_blocks = temporalConstraintBlockIds; + problem.Evaluate(evaluateOptions, &cost, NULL, NULL, NULL); + ALICEVISION_LOG_INFO("temporalConstraintBlocks cost : " << cost); + } // configure a Bundle Adjustment engine and run it // make Ceres automatically detect the bundle structure. @@ -997,6 +1135,18 @@ bool BundleAdjustmentCeres::adjust(sfmData::SfMData& sfmData, ERefineOptions ref ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); + ALICEVISION_LOG_INFO("landmarksBlockIds : " << landmarksBlockIds.size()); + evaluateOptions.residual_blocks = landmarksBlockIds; + problem.Evaluate(evaluateOptions, &cost, NULL, NULL, NULL); + ALICEVISION_LOG_INFO("landmarksBlocks cost : " << cost); + + if (temporalConstraintBlockIds.size() != 0) + { + evaluateOptions.residual_blocks = temporalConstraintBlockIds; + problem.Evaluate(evaluateOptions, &cost, NULL, NULL, NULL); + ALICEVISION_LOG_INFO("temporalConstraintBlocks cost : " << cost); + } + // print summary if (_ceresOptions.summary) ALICEVISION_LOG_INFO(summary.FullReport()); diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp index 077db88dde..1bf0c3b80b 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp @@ -37,8 +37,7 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback { CeresOptions(bool verbose = true, bool multithreaded = true, unsigned int maxIterations = 50) : verbose(verbose), - nbThreads(multithreaded ? omp_get_max_threads() : 1) // set number of threads, 1 if OpenMP is not enabled - , + nbThreads(multithreaded ? omp_get_max_threads() : 1), // set number of threads, 1 if OpenMP is not enabled maxNumIterations(maxIterations) { setDenseBA(); // use dense BA by default @@ -58,6 +57,7 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback bool summary = false; bool verbose = true; bool useFocalPrior = true; + aliceVision::sfm::TemporalConstraintParams temporalConstraintParams; }; /** @@ -179,7 +179,7 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback * @param[in] refineOptions The chosen refine flag * @param[out] problem The Ceres bundle adjustment problem */ - void addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); + void addLandmarksToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem, std::vector& blockIds); /** * @brief Create a residual block for each survey point according to the Ceres format @@ -213,6 +213,14 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback */ void addRotationPriorsToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); + /** + * @brief Create a residual block for each camera pose to add a temporal smoothness constraint + * @param[in] sfmData The input SfMData contains all the information about the reconstruction, notably the intrinsics + * @param[in] refineOptions The chosen refine flag + * @param[out] problem The Ceres bundle adjustment problem + */ + void addTemporalSmoothnessToProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem, std::vector& blockIds); + /** * @brief Create the Ceres bundle adjustment problem with: * - extrincics and intrinsics parameters blocks. @@ -221,7 +229,7 @@ class BundleAdjustmentCeres : public BundleAdjustment, ceres::EvaluationCallback * @param[in] refineOptions The chosen refine flag * @param[out] problem The Ceres bundle adjustment problem */ - void createProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem); + void createProblem(const sfmData::SfMData& sfmData, ERefineOptions refineOptions, ceres::Problem& problem, std::vector& landmarksBlockIds, std::vector& temporalConstraintBlockIds); /** * @brief Update The given SfMData with the solver solution diff --git a/src/aliceVision/sfm/bundle/costfunctions/temporalConstraint.hpp b/src/aliceVision/sfm/bundle/costfunctions/temporalConstraint.hpp new file mode 100644 index 0000000000..c791c2274b --- /dev/null +++ b/src/aliceVision/sfm/bundle/costfunctions/temporalConstraint.hpp @@ -0,0 +1,406 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2024 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include +#include +#include +#include +#include "dynamic_cost_function_to_functor.h" + +namespace aliceVision { +namespace sfm { + +struct TemporalConstraintFunctor +{ + explicit TemporalConstraintFunctor(const double positionWeight, const double orientationWeight, + const double c0pWeight, const double c1pWeight, const double c2pWeight, + const double c0oWeight, const double c1oWeight, const double c2oWeight, const int firstViews) + : _firstViews(firstViews) + { + _c0pWeight = c0pWeight * positionWeight; + _c1pWeight = c1pWeight * positionWeight; + _c2pWeight = c2pWeight * positionWeight; + + _c0oWeight = c0oWeight * orientationWeight; + _c1oWeight = c1oWeight * orientationWeight; + _c2oWeight = c2oWeight * orientationWeight; + } + + template + bool operator()(const T* para0, const T* para1, const T* para2, const T* para3, T* residuals) const + { + const auto viewPreProcess = [&](const T* angleAxis, const T* translation, T* viewCenter, T* quaternion, T* invQuaternion) + { + T invAngleAxis[3]; + // Inverse the rotation to compute the camera position + invAngleAxis[0] = -angleAxis[0]; + invAngleAxis[1] = -angleAxis[1]; + invAngleAxis[2] = -angleAxis[2]; + ceres::AngleAxisRotatePoint(invAngleAxis, translation, viewCenter); + ceres::AngleAxisToQuaternion(angleAxis, quaternion); + ceres::AngleAxisToQuaternion(invAngleAxis, invQuaternion); + }; + + T viewCenter0[3], viewCenter1[3], viewCenter2[3], viewCenter3[3]; + T quaternion0[4], quaternion1[4], quaternion2[4], quaternion3[4]; + T invQuaternion0[4], invQuaternion1[4], invQuaternion2[4], invQuaternion3[4]; + + viewPreProcess(¶0[0], ¶0[3], viewCenter0, quaternion0, invQuaternion0); + viewPreProcess(¶1[0], ¶1[3], viewCenter1, quaternion1, invQuaternion1); + viewPreProcess(¶2[0], ¶2[3], viewCenter2, quaternion2, invQuaternion2); + viewPreProcess(¶3[0], ¶3[3], viewCenter3, quaternion3, invQuaternion3); + + const auto computeAngleDiff = [&](const T* quaternionA, const T* invQuaternionB, T* angleAxisDiff) + { + T quaternionDiff[4]; + ceres::QuaternionProduct(quaternionA, invQuaternionB, quaternionDiff); + ceres::QuaternionToAngleAxis(quaternionDiff, angleAxisDiff); + }; + + T angleAxisDiff0[3], angleAxisDiff1[3], angleAxisDiff2[3]; + + computeAngleDiff(quaternion1, invQuaternion0, angleAxisDiff0); + computeAngleDiff(quaternion2, invQuaternion1, angleAxisDiff1); + computeAngleDiff(quaternion3, invQuaternion2, angleAxisDiff2); + + if (_firstViews == 2) + { + residuals[0] = _c0oWeight * angleAxisDiff0[0]; + residuals[1] = _c0oWeight * angleAxisDiff0[1]; + residuals[2] = _c0oWeight * angleAxisDiff0[2]; + } + else if (_firstViews == 1) + { + residuals[0] = _c0oWeight * angleAxisDiff1[0]; + residuals[1] = _c0oWeight * angleAxisDiff1[1]; + residuals[2] = _c0oWeight * angleAxisDiff1[2]; + + residuals[3] = _c1oWeight * (angleAxisDiff1[0] - angleAxisDiff0[0]); + residuals[4] = _c1oWeight * (angleAxisDiff1[1] - angleAxisDiff0[1]); + residuals[5] = _c1oWeight * (angleAxisDiff1[2] - angleAxisDiff0[2]); + } + else + { + residuals[0] = _c0oWeight * angleAxisDiff2[0]; + residuals[1] = _c0oWeight * angleAxisDiff2[1]; + residuals[2] = _c0oWeight * angleAxisDiff2[2]; + + residuals[3] = _c1oWeight * (angleAxisDiff2[0] - angleAxisDiff1[0]); + residuals[4] = _c1oWeight * (angleAxisDiff2[1] - angleAxisDiff1[1]); + residuals[5] = _c1oWeight * (angleAxisDiff2[2] - angleAxisDiff1[2]); + + residuals[6] = _c2oWeight * (2. * angleAxisDiff1[0] - angleAxisDiff0[0] - angleAxisDiff2[0]); + residuals[7] = _c2oWeight * (2. * angleAxisDiff1[1] - angleAxisDiff0[1] - angleAxisDiff2[1]); + residuals[8] = _c2oWeight * (2. * angleAxisDiff1[2] - angleAxisDiff0[2] - angleAxisDiff2[2]); + } + + + if (_firstViews == 2) + { + residuals[3] = _c0pWeight * (viewCenter1[0] - viewCenter0[0]); + residuals[4] = _c0pWeight * (viewCenter1[1] - viewCenter0[1]); + residuals[5] = _c0pWeight * (viewCenter1[2] - viewCenter0[2]); + } + else if (_firstViews == 1) + { + residuals[6] = _c0pWeight * (viewCenter2[0] - viewCenter1[0]); + residuals[7] = _c0pWeight * (viewCenter2[1] - viewCenter1[1]); + residuals[8] = _c0pWeight * (viewCenter2[2] - viewCenter1[2]); + + T c1normalization = sqrt( (viewCenter2[0] - viewCenter1[0]) * (viewCenter2[0] - viewCenter1[0]) + + (viewCenter2[1] - viewCenter1[1]) * (viewCenter2[1] - viewCenter1[1]) + + (viewCenter2[2] - viewCenter1[2]) * (viewCenter2[2] - viewCenter1[2]) + + (viewCenter1[0] - viewCenter0[0]) * (viewCenter1[0] - viewCenter0[0]) + + (viewCenter1[1] - viewCenter0[1]) * (viewCenter1[1] - viewCenter0[1]) + + (viewCenter1[2] - viewCenter0[2]) * (viewCenter1[2] - viewCenter0[2]) ); + + residuals[9] = _c1pWeight * (2. * viewCenter1[0] - viewCenter0[0] - viewCenter2[0]) / c1normalization; + residuals[10] = _c1pWeight * (2. * viewCenter1[1] - viewCenter0[1] - viewCenter2[1]) / c1normalization; + residuals[11] = _c1pWeight * (2. * viewCenter1[2] - viewCenter0[2] - viewCenter2[2]) / c1normalization; + } + else + { + residuals[9] = _c0pWeight * (viewCenter3[0] - viewCenter2[0]); + residuals[10] = _c0pWeight * (viewCenter3[1] - viewCenter2[1]); + residuals[11] = _c0pWeight * (viewCenter3[2] - viewCenter2[2]); + + T c1normalization = sqrt( (viewCenter3[0] - viewCenter2[0]) * (viewCenter3[0] - viewCenter2[0]) + + (viewCenter3[1] - viewCenter2[1]) * (viewCenter3[1] - viewCenter2[1]) + + (viewCenter3[2] - viewCenter2[2]) * (viewCenter3[2] - viewCenter2[2]) + + (viewCenter2[0] - viewCenter1[0]) * (viewCenter2[0] - viewCenter1[0]) + + (viewCenter2[1] - viewCenter1[1]) * (viewCenter2[1] - viewCenter1[1]) + + (viewCenter2[2] - viewCenter1[2]) * (viewCenter2[2] - viewCenter1[2]) ); + + residuals[12] = _c1pWeight * (2. * viewCenter2[0] - viewCenter1[0] - viewCenter3[0]) / c1normalization; + residuals[13] = _c1pWeight * (2. * viewCenter2[1] - viewCenter1[1] - viewCenter3[1]) / c1normalization; + residuals[14] = _c1pWeight * (2. * viewCenter2[2] - viewCenter1[2] - viewCenter3[2]) / c1normalization; + + T c2normalization = sqrt( (viewCenter3[0] - viewCenter2[0]) * (viewCenter3[0] - viewCenter2[0]) + + (viewCenter3[1] - viewCenter2[1]) * (viewCenter3[1] - viewCenter2[1]) + + (viewCenter3[2] - viewCenter2[2]) * (viewCenter3[2] - viewCenter2[2]) + + (viewCenter2[0] - viewCenter1[0]) * (viewCenter2[0] - viewCenter1[0]) + + (viewCenter2[1] - viewCenter1[1]) * (viewCenter2[1] - viewCenter1[1]) + + (viewCenter2[2] - viewCenter1[2]) * (viewCenter2[2] - viewCenter1[2]) + + (viewCenter1[0] - viewCenter0[0]) * (viewCenter1[0] - viewCenter0[0]) + + (viewCenter1[1] - viewCenter0[1]) * (viewCenter1[1] - viewCenter0[1]) + + (viewCenter1[2] - viewCenter0[2]) * (viewCenter1[2] - viewCenter0[2]) ); + + residuals[15] = _c2pWeight * (3. * viewCenter1[0] - 3. * viewCenter2[0] + viewCenter3[0] - viewCenter0[0]) / c2normalization; + residuals[16] = _c2pWeight * (3. * viewCenter1[1] - 3. * viewCenter2[1] + viewCenter3[1] - viewCenter0[1]) / c2normalization; + residuals[17] = _c2pWeight * (3. * viewCenter1[2] - 3. * viewCenter2[2] + viewCenter3[2] - viewCenter0[2]) / c2normalization; + } + + return true; + } + + /** + * @brief Create the appropriate cost functor + * @return cost functor + */ + inline static ceres::CostFunction* createCostFunction(const double positionWeight, const double orientationWeight, + const double c0pWeight, const double c1pWeight, const double c2pWeight, + const double c0oWeight, const double c1oWeight, const double c2oWeight, const int firstViews = 0) + { + if (firstViews == 2) + { + return new ceres::AutoDiffCostFunction + (new TemporalConstraintFunctor(positionWeight, orientationWeight, + c0pWeight, c1pWeight, c2pWeight, c0oWeight, c1oWeight, c2oWeight, 2)); + } + else if (firstViews == 1) + { + return new ceres::AutoDiffCostFunction + (new TemporalConstraintFunctor(positionWeight, orientationWeight, + c0pWeight, c1pWeight, c2pWeight, c0oWeight, c1oWeight, c2oWeight, 1)); + } + else + { + return new ceres::AutoDiffCostFunction + (new TemporalConstraintFunctor(positionWeight, orientationWeight, + c0pWeight, c1pWeight, c2pWeight, c0oWeight, c1oWeight, c2oWeight, 0)); + } + } + + double _c0oWeight; + double _c1oWeight; + double _c2oWeight; + double _c0pWeight; + double _c1pWeight; + double _c2pWeight; + + int _firstViews; +}; + + +struct TemporalRegularizationFunctor +{ + explicit TemporalRegularizationFunctor(const double regularizationWeight, const double trackSquareLength, const int paraSize) + : _regWeight(regularizationWeight), + _squareLength(trackSquareLength), + _paraSize(paraSize) + { + } + + template + bool operator()(const T* const * para, T* residuals) const + { + residuals[0] = _regWeight * (sqrt( (para[1][3] - para[0][3]) * (para[1][3] - para[0][3]) + + (para[1][4] - para[0][4]) * (para[1][4] - para[0][4]) + + (para[1][5] - para[0][5]) * (para[1][5] - para[0][5]) ) - _squareLength); + + for (int paraIdx = 2; paraIdx < _paraSize; paraIdx++) + { + residuals[0] += _regWeight * sqrt( (para[paraIdx][3] - para[paraIdx-1][3]) * (para[paraIdx][3] - para[paraIdx-1][3]) + + (para[paraIdx][4] - para[paraIdx-1][4]) * (para[paraIdx][4] - para[paraIdx-1][4]) + + (para[paraIdx][5] - para[paraIdx-1][5]) * (para[paraIdx][5] - para[paraIdx-1][5]) ); + } + + return true; + } + + /** + * @brief Create the appropriate cost functor + * @return cost functor + */ + inline static ceres::CostFunction* createCostFunction(const double regularizationWeight, + const double trackSquareLength, const int paraSize) + { + auto costFunction = new ceres::DynamicAutoDiffCostFunction + (new TemporalRegularizationFunctor(regularizationWeight, trackSquareLength, paraSize)); + + for (int paraIdx = 0; paraIdx < paraSize; paraIdx++) + { + costFunction->AddParameterBlock(6); + } + costFunction->SetNumResiduals(1); + + return costFunction; + } + + int _paraSize; + double _regWeight; + double _squareLength; +}; + + +struct Landmarks2viewsRegularizationFunctor +{ + explicit Landmarks2viewsRegularizationFunctor(const double regularizationWeight, const std::vector landmarks, const std::vector meanL2V, const int paraSize) + : _regWeight(regularizationWeight), + _landmarks(landmarks), + _meanL2V(meanL2V), + _paraSize(paraSize) + { + } + + template + bool operator()(const T* const * para, T* residuals) const + { + double landmarksMean0 = 0.; + double landmarksMean1 = 0.; + double landmarksMean2 = 0.; + + double landmarksCount = double(_landmarks.size()); + + for (double* landmark : _landmarks) + { + landmarksMean0 += landmark[0]; + landmarksMean1 += landmark[1]; + landmarksMean2 += landmark[2]; + } + + T landmarksMean0T = static_cast ( landmarksMean0 / landmarksCount ); + T landmarksMean1T = static_cast ( landmarksMean1 / landmarksCount ); + T landmarksMean2T = static_cast ( landmarksMean2 / landmarksCount ); + + T posesMean0 = para[0][3]; + T posesMean1 = para[0][4]; + T posesMean2 = para[0][5]; + + for (int paraIdx = 1; paraIdx < _paraSize; paraIdx++) + { + posesMean0 += para[paraIdx][3]; + posesMean1 += para[paraIdx][4]; + posesMean2 += para[paraIdx][5]; + } + + T posesCount = static_cast(double(_paraSize)); + + posesMean0 /= posesCount; + posesMean1 /= posesCount; + posesMean2 /= posesCount; + + residuals[0] = _regWeight * (posesMean0 - landmarksMean0T - static_cast(_meanL2V[0])); + residuals[1] = _regWeight * (posesMean1 - landmarksMean1T - static_cast(_meanL2V[1])); + residuals[2] = _regWeight * (posesMean2 - landmarksMean2T - static_cast(_meanL2V[2])); + + return true; + } + + /** + * @brief Create the appropriate cost functor + * @return cost functor + */ + inline static ceres::CostFunction* createCostFunction(const double regularizationWeight, + const std::vector landmarks, const std::vector meanL2V, const int paraSize) + { + auto costFunction = new ceres::DynamicAutoDiffCostFunction + (new Landmarks2viewsRegularizationFunctor(regularizationWeight, landmarks, meanL2V, paraSize)); + + for (int paraIdx = 0; paraIdx < paraSize; paraIdx++) + { + costFunction->AddParameterBlock(6); + } + costFunction->SetNumResiduals(3); + + return costFunction; + } + + int _paraSize; + double _regWeight; + std::vector _landmarks; + std::vector _meanL2V; +}; + + +const double meanFocalLength(const sfmData::SfMData& sfmData) +{ + std::vector focalLengths; + + for (const auto& [intrinsicId, intrinsic] : sfmData.getIntrinsics()) + { + const double focalLengthPixels = intrinsic->getParameters().at(0); + const double focalLength = focalLengthPixels * intrinsic->sensorWidth() / double(intrinsic->w()); + focalLengths.push_back(focalLength); + } + + if (focalLengths.empty()) + { + return 1.; + } + + return std::accumulate(focalLengths.begin(), focalLengths.end(), 0.) / focalLengths.size(); +} + + +const double cameraTrackSquareLength(const std::vector poses) +{ + double trackLength = 0.; + + const int viewCount = poses.size(); + + for (int frameIdx = 1; frameIdx < viewCount; frameIdx++) + { + trackLength += sqrt( (poses[frameIdx][3] - poses[frameIdx-1][3]) * (poses[frameIdx][3] - poses[frameIdx-1][3]) + + (poses[frameIdx][4] - poses[frameIdx-1][4]) * (poses[frameIdx][4] - poses[frameIdx-1][4]) + + (poses[frameIdx][5] - poses[frameIdx-1][5]) * (poses[frameIdx][5] - poses[frameIdx-1][5]) ); + } + + return trackLength; +} + +void meanLandmarks2viewsPose(const std::vector landmarks, const std::vector poses, std::vector& meanL2V) +{ + const double landmarksCount = double(landmarks.size()); + + std::vector landmarksMean(3, 0.); + + for (double* landmark : landmarks) + { + landmarksMean[0] += landmark[0]; + landmarksMean[1] += landmark[1]; + landmarksMean[2] += landmark[2]; + } + + landmarksMean[0] /= landmarksCount; + landmarksMean[1] /= landmarksCount; + landmarksMean[2] /= landmarksCount; + + const double posesCount = double(poses.size()); + + std::vector posesMean(3, 0.); + + for (double* pose : poses) + { + posesMean[0] += pose[3]; + posesMean[1] += pose[4]; + posesMean[2] += pose[5]; + } + + posesMean[0] /= posesCount; + posesMean[1] /= posesCount; + posesMean[2] /= posesCount; + + meanL2V[0] = posesMean[0] - landmarksMean[0]; + meanL2V[1] = posesMean[1] - landmarksMean[1]; + meanL2V[2] = posesMean[2] - landmarksMean[2]; +} + +} // namespace sfm +} // namespace aliceVision diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp index 1e162d07b0..1b3b2555f2 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.cpp @@ -8,19 +8,20 @@ #include #include +#include #include namespace aliceVision { namespace sfm { bool SfmBundle::process(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds) -{ +{ ALICEVISION_LOG_INFO("SfmBundle::process start"); BundleAdjustmentCeres::CeresOptions options; BundleAdjustment::ERefineOptions refineOptions; - refineOptions |= BundleAdjustment::REFINE_ROTATION; + refineOptions |= BundleAdjustment::REFINE_ROTATION; refineOptions |= BundleAdjustment::REFINE_TRANSLATION; if (_isStructureRefinementEnabled) @@ -30,17 +31,35 @@ bool SfmBundle::process(sfmData::SfMData & sfmData, const track::TracksHandler & refineOptions |= BundleAdjustment::REFINE_INTRINSICS_ALL; + if (_bundleTemporalConstraint) + { + refineOptions |= BundleAdjustment::REFINE_TEMPORAL_SMOOTHNESS_CONSTRAINT; + options.temporalConstraintParams = _tempConstrParams; + + // Fill in the blanks in the views by interpolating new poses (position and orientation) + + sfm::poseFilter poseFilter; + if (!poseFilter.interpolateMissingPoses(sfmData, false)) + return false; + + if (_bundleExitAfterPoseInterpolation) + { + ALICEVISION_LOG_INFO("SfmBundle::process ended after pose interpolation"); + return true; + } + } + options.setSparseBA(); BundleAdjustmentCeres bundleObject(options, _minNbCamerasToRefinePrincipalPoint); //Repeat until nothing change - do + do { if (!initializeIteration(sfmData, tracksHandler, viewIds)) { return false; } - + const bool success = bundleObject.adjust(sfmData, refineOptions); if (!success) { @@ -67,7 +86,7 @@ bool SfmBundle::cleanup(sfmData::SfMData & sfmData) // Remove poses without enough observations in an interactive fashion const std::size_t nbOutliers = nbOutliersResidualErr + nbOutliersAngleErr; std::set removedViewsIdIteration; - bool somethingErased = eraseUnstablePosesAndObservations(sfmData, _minPointsPerPose, _minTrackLength, &removedViewsIdIteration); + bool somethingErased = !_bundleTemporalConstraint && eraseUnstablePosesAndObservations(sfmData, _minPointsPerPose, _minTrackLength, &removedViewsIdIteration); bool somethingChanged = /*somethingErased || */(nbOutliers > _bundleAdjustmentMaxOutlier) || (nbOutliersConstraints > 0); diff --git a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp index 5358d87fbd..90c31f95ec 100644 --- a/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp +++ b/src/aliceVision/sfm/pipeline/expanding/SfmBundle.hpp @@ -20,7 +20,7 @@ class SfmBundle { public: using uptr = std::unique_ptr; - + public: /** * @brief Process bundle @@ -45,6 +45,13 @@ class SfmBundle _bundleAdjustmentMaxOutlier = bundleAdjustmentMaxOutlier; } + void setTemporalConstraintParams(aliceVision::sfm::TemporalConstraintParams tempConstrParams, bool bundleTemporalConstraint, bool bundleExitAfterPoseInterpolation) + { + _tempConstrParams = tempConstrParams; + _bundleTemporalConstraint = bundleTemporalConstraint; + _bundleExitAfterPoseInterpolation = bundleExitAfterPoseInterpolation; + } + /** * @brief set the minimal allowed parallax degree after bundle (Or the landmark will be removed) * @param angle the angle in DEGREES @@ -91,7 +98,7 @@ class SfmBundle bool initializeIteration(sfmData::SfMData & sfmData, const track::TracksHandler & tracksHandler, const std::set & viewIds); /** - * Cleanup sfmData + * Cleanup sfmData * @param sfmData the scene to clean * @return true if enough change occurred during the cleaning */ @@ -111,6 +118,9 @@ class SfmBundle size_t _bundleAdjustmentMaxOutlier = 50; size_t _minNbCamerasToRefinePrincipalPoint = 3; bool _useLBA = true; + bool _bundleTemporalConstraint = false; + bool _bundleExitAfterPoseInterpolation = false; + aliceVision::sfm::TemporalConstraintParams _tempConstrParams; size_t _minNbCamerasLBA = 100; size_t _LBAGraphDistanceLimit = 1; size_t _LBAMinNbOfMatches = 50; diff --git a/src/aliceVision/sfm/utils/poseFilter.cpp b/src/aliceVision/sfm/utils/poseFilter.cpp index 1805bc8b29..6a6f0b309a 100644 --- a/src/aliceVision/sfm/utils/poseFilter.cpp +++ b/src/aliceVision/sfm/utils/poseFilter.cpp @@ -75,6 +75,133 @@ bool poseFilter::process(sfmData::SfMData& sfmData, const bool filterPosition, c } +bool poseFilter::interpolateMissingPoses(sfmData::SfMData& sfmData, const bool ignoreFirstAndLast) +{ + using namespace Eigen; + + ALICEVISION_LOG_INFO("poseFilter::interpolateMissingPoses start"); + + const int viewCount = sfmData.getViews().size(); + + std::vector viewIdsVec(viewCount); + + IndexT firstViewFrameId = sfmData.getViews().begin()->second->getFrameId(); + IndexT minFrameId = firstViewFrameId; // Arbitrary frameId init + IndexT maxFrameId = firstViewFrameId; // Arbitrary frameId init + IndexT minFrameIdWithPose; + IndexT maxFrameIdWithPose; + + bool existingPoseFound = false; + + // Get the frameIDs range and the frameID of the first and last views with an existing pose + for (const auto& [viewID, viewPtr] : sfmData.getViews()) + { + const IndexT frameId = viewPtr->getFrameId(); + if (frameId < minFrameId) + { + minFrameId = frameId; + } + if (frameId > maxFrameId) + { + maxFrameId = frameId; + } + if ( sfmData.existsPose(*viewPtr) ) + { + if (!existingPoseFound || frameId < minFrameIdWithPose) + minFrameIdWithPose = frameId; + + if (!existingPoseFound || frameId > maxFrameIdWithPose) + maxFrameIdWithPose = frameId; + + existingPoseFound = true; + } + } + + const int frameIdRange = maxFrameId - minFrameId + 1; + + if ( !existingPoseFound || (frameIdRange != viewCount) ) + { + return false; + } + + // Store the temporally ordered view IDs + for (const auto& [viewID, viewPtr] : sfmData.getViews()) + { + const IndexT frameId = viewPtr->getFrameId(); + viewIdsVec[frameId-minFrameId] = viewID; + + if (!sfmData.existsPose(*viewPtr)) + ALICEVISION_LOG_INFO(" frameId without pose : " << frameId); + } + + const sfmData::View& lastValidView = sfmData.getView(viewIdsVec[minFrameIdWithPose-minFrameId]); + sfmData::CameraPose lastValidPose = sfmData.getPose(lastValidView); + + VectorX missingPosesMask(viewCount); + missingPosesMask.setZero(); + + // Initialize the pose of the views without pose + for (int frameIdx = 0; frameIdx < viewCount; frameIdx++) + { + IndexT frameViewID = viewIdsVec[frameIdx]; + const sfmData::View& currentView = sfmData.getView(frameViewID); + + if (!sfmData.existsPose(currentView)) + { + // Create a binary mask to be able to restore the original values + missingPosesMask(frameIdx) = true; + + if (!ignoreFirstAndLast || (minFrameIdWithPose < (frameIdx + minFrameId) && (frameIdx + minFrameId) < maxFrameIdWithPose) ) + sfmData.setPose(currentView, lastValidPose); + } + else + lastValidPose = sfmData.getPose(currentView); + } + + tempFilter tFilter; + + tFilter.init(); + + MatrixXd viewRotations(4, viewCount); + MatrixXd viewCenters(3, viewCount); + + // Get the temporally ordered view positions and orientations + for (int frameIdx = 0; frameIdx < viewCount; frameIdx++) + { + const sfmData::View& frameView = sfmData.getView(viewIdsVec[frameIdx]); + const sfmData::CameraPose framePose = sfmData.getPose(frameView); + viewCenters.col(frameIdx) = framePose.getTransform().center(); + AngleAxisd aa(framePose.getTransform().rotation()); + viewRotations.col(frameIdx) << aa.angle(), aa.axis(); + } + + int scaleFactor = 8; + int iterationCount = 1000; + + // Apply a temporal filter to view positions + viewCenters = tFilter.applyMultiscale(viewCenters, scaleFactor, iterationCount, false, missingPosesMask); + + // Apply a temporal filter to view orientations + viewRotations = tFilter.applyMultiscale(viewRotations, scaleFactor, iterationCount, true, missingPosesMask); + + // Save the temporally filtered poses + for (int frameIdx = 0; frameIdx < viewCount; frameIdx++) + { + if (!missingPosesMask(frameIdx)) + continue; + + IndexT frameViewID = viewIdsVec[frameIdx]; + AngleAxisd aa(viewRotations(0, frameIdx), viewRotations(seqN(1,3), frameIdx)); + geometry::Pose3 newPose(aa.toRotationMatrix(), viewCenters.col(frameIdx)); + const sfmData::View& frameView = sfmData.getView(frameViewID); + sfmData.setPose(frameView, sfmData::CameraPose(newPose)); + } + + ALICEVISION_LOG_INFO("poseFilter::interpolateMissingPoses end"); + return true; +} + + bool poseFilter::getOrderedViewIds(sfmData::SfMData& sfmData, std::vector& viewIdsVec) { const int viewCount = sfmData.getViews().size(); @@ -119,6 +246,9 @@ bool poseFilter::getOrderedViewIds(sfmData::SfMData& sfmData, std::vectorgetFrameId(); viewIdsVec[frameId-minFrameId] = viewID; + + if (!sfmData.existsPose(*viewPtr)) + ALICEVISION_LOG_INFO(" frameId without pose : " << frameId); } ALICEVISION_LOG_DEBUG(" minFrameIdWithPose : " << minFrameIdWithPose); @@ -143,6 +273,82 @@ bool poseFilter::getOrderedViewIds(sfmData::SfMData& sfmData, std::vector& poseIdsVec, IndexT& firstViewWithPose, IndexT& lastViewWithPose) +{ + std::map> candidateViewIdLists; + std::map> candidateFrameIdLists; + + // Group views by common filename pattern + for (const auto& [viewID, viewPtr] : sfmData.getViews()) + { + const std::filesystem::path imagePath = std::filesystem::path(viewPtr->getImage().getImagePath()); + std::string parentPath = imagePath.parent_path().string(); + // Remove every digit in the filename + std::string filePattern = std::regex_replace(imagePath.filename().string(), std::regex("\\d"), ""); + candidateViewIdLists[parentPath+filePattern].push_back(viewID); + candidateFrameIdLists[parentPath+filePattern].push_back(viewPtr->getFrameId()); + } + + IndexT minFrameId; + + int longestValidRange = 1; + std::string longestValidList; + + // Select the longest list of views having no hole in the frameIds range + for (const auto& [filePattern, frameIDs] : candidateFrameIdLists) + { + auto [listMin, listMax] = std::minmax_element(frameIDs.begin(), frameIDs.end()); + auto listRange = *listMax + 1 - *listMin; + bool noHole = frameIDs.size() == listRange; + if (noHole && listRange > longestValidRange) + { + longestValidRange = listRange; + longestValidList = filePattern; + minFrameId = *listMin; + } + } + + if (longestValidRange == 1) + return false; + + poseIdsVec.resize(longestValidRange); + + bool existingPoseFound = false; + IndexT minFrameIdWithPose; + IndexT maxFrameIdWithPose; + + // Get the frameID of the first and last views with an existing pose + // And store the temporally ordered poseIDs + for (const auto& viewID : candidateViewIdLists[longestValidList]) + { + const sfmData::View& view = sfmData.getView(viewID); + + if ( sfmData.existsPose(view) ) + { + const IndexT frameId = view.getFrameId(); + + if (!existingPoseFound || frameId < minFrameIdWithPose) + minFrameIdWithPose = frameId; + + if (!existingPoseFound || frameId > maxFrameIdWithPose) + maxFrameIdWithPose = frameId; + + existingPoseFound = true; + + poseIdsVec[frameId-minFrameId] = view.getPoseId(); + } + } + + if (!existingPoseFound) + return false; + + firstViewWithPose = minFrameIdWithPose - minFrameId; + lastViewWithPose = maxFrameIdWithPose - minFrameId; + + return true; +} + } // namespace sfm } // namespace aliceVision @@ -338,7 +544,7 @@ Eigen::MatrixXd tempFilter::apply(Eigen::MatrixXd& inputSignal, bool isAngle) } -Eigen::MatrixXd tempFilter::applyMultiscale(Eigen::MatrixXd& inputSignal, const unsigned int scaleFactor, const int iterationCount, bool isAngle) +Eigen::MatrixXd tempFilter::applyMultiscale(Eigen::MatrixXd& inputSignal, const unsigned int scaleFactor, const int iterationCount, bool isAngle, const Eigen::VectorX& posesMask) { using namespace Eigen; using namespace indexing; @@ -351,6 +557,10 @@ Eigen::MatrixXd tempFilter::applyMultiscale(Eigen::MatrixXd& inputSignal, const // controlled by the following constant const double SCALE_REDUCTION_FACTOR = 1.4; + // An optional mask can be used to apply the filter just on some poses + bool useMask = posesMask.cols() != 0; + MatrixX mask; + for (int scaleF = scaleFactor; scaleF >= 1; scaleF = (scaleF > 1) ? round(double(scaleF) / SCALE_REDUCTION_FACTOR) : 0) { ALICEVISION_LOG_DEBUG(" Filter scale factor : " << scaleF); @@ -361,9 +571,19 @@ Eigen::MatrixXd tempFilter::applyMultiscale(Eigen::MatrixXd& inputSignal, const for (int phase = 0; phase < scaleF; phase++) { MatrixXd scaledSignal(filteredSignal(all, seq(phase, last, scaleF))); + + if (useMask) + mask = posesMask(seq(phase, last, scaleF)).rowwise().replicate(inputSignal.rows()).transpose(); + for (int iterFilter = 0; iterFilter < iterationCount; iterFilter++) { scaledSignal = apply(scaledSignal, isAngle); + + if (useMask) + { + // Restore the original poses + scaledSignal = mask.select(scaledSignal, inputSignal(all, seq(phase, last, scaleF))); + } } filteredSignal(all, seq(phase, last, scaleF)) = scaledSignal; } diff --git a/src/aliceVision/sfm/utils/poseFilter.hpp b/src/aliceVision/sfm/utils/poseFilter.hpp index a5ec050231..ec9eb33bdf 100644 --- a/src/aliceVision/sfm/utils/poseFilter.hpp +++ b/src/aliceVision/sfm/utils/poseFilter.hpp @@ -28,11 +28,13 @@ class poseFilter * @return false if an error occurred */ bool process(sfmData::SfMData& sfmData, const bool filterPosition, const bool filterRotation, const int scaleFactor, const int iterationCount); + bool interpolateMissingPoses(sfmData::SfMData& sfmData, const bool ignoreFirstAndLast); private: bool getOrderedViewIds(sfmData::SfMData& sfmData, std::vector& viewIdsVec); }; +bool getOrderedPoseIds(const sfmData::SfMData& sfmData, std::vector& poseIdsVec, IndexT& firstViewWithPose, IndexT& lastViewWithPose); } // namespace sfm } // namespace aliceVision @@ -43,7 +45,7 @@ class tempFilter bool init(); bool applyCoreFilter(Eigen::MatrixXd& inputSignal, Eigen::MatrixXd& filteredSignal, bool diffSignal); Eigen::MatrixXd apply(Eigen::MatrixXd& inputSignal, bool isAngle); - Eigen::MatrixXd applyMultiscale(Eigen::MatrixXd& inputSignal, const unsigned int scaleFactor, const int iterationCount, bool isAngle); + Eigen::MatrixXd applyMultiscale(Eigen::MatrixXd& inputSignal, const unsigned int scaleFactor, const int iterationCount, bool isAngle, const Eigen::VectorX& posesMask=Eigen::VectorX(0)); private: bool initialized; diff --git a/src/aliceVision/sfmDataIO/sceneSample.cpp b/src/aliceVision/sfmDataIO/sceneSample.cpp index e277dd8b3d..4e812582a4 100644 --- a/src/aliceVision/sfmDataIO/sceneSample.cpp +++ b/src/aliceVision/sfmDataIO/sceneSample.cpp @@ -92,10 +92,10 @@ void generateCubeScene(sfmData::SfMData& output) const double offsetX = -26; const double offsetY = 16; output.getIntrinsics().emplace( - 0, camera::createPinhole(camera::EDISTORTION::DISTORTION_NONE, camera::EUNDISTORTION::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY)); + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY)); output.getIntrinsics().emplace( 1, - camera::createPinhole(camera::EDISTORTION::DISTORTION_RADIALK3, camera::EUNDISTORTION::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY, {0.1, 0.05, -0.001})); + camera::createPinhole(camera::DISTORTION_RADIALK3, camera::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY, {0.1, 0.05, -0.001})); // Generate poses on another cube IndexT idpose = 0; @@ -128,22 +128,13 @@ void generateCubeScene(sfmData::SfMData& output) void generateSphereScene(sfmData::SfMData& output, int pointsNb, int posesNb) { // Generate random points on a sphere - IndexT idpt = 0; - for (int pt = 0; pt < pointsNb; pt++) - { - Eigen::Vector3d point3D = Eigen::Vector3d::Random(); - point3D = 1. * point3D / point3D.norm(); - - output.getLandmarks().emplace(idpt, sfmData::Landmark(point3D, feature::EImageDescriberType::UNKNOWN)); - ++idpt; - } const int w = 4092; const int h = 2048; - const double focalLengthPixX = 1000.0; - const double focalLengthPixY = 2000.0; + const double focalLengthPixX = 10000.0; + const double focalLengthPixY = 20000.0; output.getIntrinsics().emplace( - 0, camera::createPinhole(camera::EDISTORTION::DISTORTION_NONE, camera::EUNDISTORTION::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, 0, 0)); + 0, camera::createPinhole(camera::DISTORTION_NONE, camera::UNDISTORTION_NONE, w, h, focalLengthPixX, focalLengthPixY, 0, 0)); // Generate poses on a circle for (IndexT idPV = 0; idPV < posesNb; idPV++) @@ -157,6 +148,25 @@ void generateSphereScene(sfmData::SfMData& output, int pointsNb, int posesNb) output.getViews().emplace(idPV, std::make_shared("", idPV, 0, idPV, w, h)); output.getView(idPV).setFrameId(idPV); } + + // Generate random points on a sphere and corresponding observations in each view + const double arbitraryScale = 1.0; + for (int landmarkId = 0; landmarkId < pointsNb; landmarkId++) + { + Eigen::Vector3d point3D = Eigen::Vector3d::Random(); + point3D = point3D / point3D.norm(); + + sfmData::Landmark landmark(point3D, feature::EImageDescriberType::UNKNOWN); + for (int viewId = 0; viewId < posesNb; viewId++) + { + const sfmData::View& view = *output.getViews().at(viewId); + const geometry::Pose3 pose = output.getPose(view).getTransform(); + const camera::IntrinsicBase& intrinsic = *output.getIntrinsics().at(0); + const Eigen::Vector2d pt = intrinsic.transformProject(pose, landmark.getX().homogeneous(), true); + landmark.getObservations().emplace(viewId, sfmData::Observation(pt, landmarkId, arbitraryScale)); + } + output.getLandmarks().emplace(landmarkId, landmark); + } } } // namespace sfmDataIO diff --git a/src/software/pipeline/main_sfmExpanding.cpp b/src/software/pipeline/main_sfmExpanding.cpp index 0c11f5e1e1..31200e9b7b 100644 --- a/src/software/pipeline/main_sfmExpanding.cpp +++ b/src/software/pipeline/main_sfmExpanding.cpp @@ -35,7 +35,7 @@ using namespace aliceVision; namespace po = boost::program_options; -//This intermediate class is used as a proxy to not link +//This intermediate class is used as a proxy to not link //sfm with mesh library class MeshPointFetcher : public sfm::PointFetcher { @@ -64,11 +64,11 @@ class MeshPointFetcher : public sfm::PointFetcher * @param normal result normal in some global coordinates frame * @param intrinsic the camera intrinsic object * @param imageCoords the input image pixel coordinates in 2D. - * @return false on error + * @return false on error */ - bool pickPointAndNormal(Vec3 & point, - Vec3 & normal, - const camera::IntrinsicBase & intrinsic, + bool pickPointAndNormal(Vec3 & point, + Vec3 & normal, + const camera::IntrinsicBase & intrinsic, const Vec2 & imageCoords) override { return _mi.pickPointAndNormal(point, normal, intrinsic, imageCoords); @@ -91,6 +91,8 @@ int aliceVision_main(int argc, char** argv) double localizerEstimatorError = 0.0; bool lockScenePreviouslyReconstructed = false; bool useLocalBA = true; + bool useTemporalConstraint = false; + bool exitAfterPoseInterpolation = false; int lbaDistanceLimit = 1; std::size_t nbFirstUnstableCameras = 30; std::size_t maxImagesPerGroup = 30; @@ -111,6 +113,8 @@ int aliceVision_main(int argc, char** argv) int randomSeed = std::mt19937::default_seed; + aliceVision::sfm::TemporalConstraintParams tempConstrParams; + // clang-format off po::options_description requiredParams("Required parameters"); requiredParams.add_options() @@ -127,6 +131,17 @@ int aliceVision_main(int argc, char** argv) ("ignoreMultiviewOnPrior", po::value(&ignoreMultiviewOnPrior)->default_value(ignoreMultiviewOnPrior),"Favour the prior based 3d reconstruction over the multiview reconstruction.") ("lockScenePreviouslyReconstructed", po::value(&lockScenePreviouslyReconstructed)->default_value(lockScenePreviouslyReconstructed),"Lock/Unlock scene previously reconstructed.") ("useLocalBA,l", po::value(&useLocalBA)->default_value(useLocalBA), "Enable/Disable the Local bundle adjustment strategy.\n It reduces the reconstruction time, especially for big datasets (500+ images).") + ("useTemporalConstraint", po::value(&useTemporalConstraint)->default_value(useTemporalConstraint), "Enable/Disable the temporal smoothness constraint to the bundle adjustment.") + ("exitAfterPoseInterpolation", po::value(&exitAfterPoseInterpolation)->default_value(exitAfterPoseInterpolation), "Exit after pose interpolation, before bundle adjustment. (for debug)") + ("tscPositionWeight", po::value(&tempConstrParams.positionWeight)->default_value(tempConstrParams.positionWeight), "Temporal Constraint Position Weight. Controls the weight of the temporal constraint applied to camera positions. Higher values enforce smoother camera path.") + ("tscOrientationWeight", po::value(&tempConstrParams.orientationWeight)->default_value(tempConstrParams.orientationWeight), "Temporal Constraint Orientation Weight. Controls the weight of the temporal constraint applied to camera orientations. Higher values enforce smoother camera rotation.") + ("tscRegularizationWeight", po::value(&tempConstrParams.regularizationWeight)->default_value(tempConstrParams.regularizationWeight), "Temporal Constraint Regularization Weight.") + ("tscC0positionWeight", po::value(&tempConstrParams.c0positionWeight)->default_value(tempConstrParams.c0positionWeight), "Temporal Constraint C0 Position Weight. Controls the weight of the continuity constraint on camera positions in the temporal constraint. Higher values enforce smoother transitions in position, reducing abrupt changes of position.") + ("tscC1positionWeight", po::value(&tempConstrParams.c1positionWeight)->default_value(tempConstrParams.c1positionWeight), "Temporal Constraint C1 Position Weight. Controls the weight of the first derivative of camera position in the temporal constraint. Higher values enforce continuity of the camera velocity, reducing abrupt changes of velocity.") + ("tscC2positionWeight", po::value(&tempConstrParams.c2positionWeight)->default_value(tempConstrParams.c2positionWeight), "Temporal Constraint C2 Position Weight: Controls the weight of the second derivative of camera position in the temporal constraint. Higher values enforce continuity of the camera acceleration, reducing abrupt changes of acceleration.") + ("tscC0orientationWeight", po::value(&tempConstrParams.c0orientationWeight)->default_value(tempConstrParams.c0orientationWeight), "Temporal Constraint C0 Orientation Weight. Controls the weight of the continuity constraint on camera orientation in the temporal constraint. Higher values enforce smoother transitions, reducing abrupt changes of the camera orientation.") + ("tscC1orientationWeight", po::value(&tempConstrParams.c1orientationWeight)->default_value(tempConstrParams.c1orientationWeight), "Temporal Constraint C1 Orientation Weight. Controls the weight of the first derivative of camera orientation in the temporal constraint. Higher values enforce continuity of the rotation velocity, reducing abrupt changes of rotation velocity.") + ("tscC2orientationWeight", po::value(&tempConstrParams.c2orientationWeight)->default_value(tempConstrParams.c2orientationWeight), "Temporal Constraint C2 Orientation Weight. Controls the weight of the second derivative of camera orientation in the temporal constraint. Higher values enforce continuity of the rotation acceleration, reducing abrupt changes of rotation acceleration.") ("localBAGraphDistance", po::value(&lbaDistanceLimit)->default_value(lbaDistanceLimit), "Graph-distance limit setting the Active region in the Local Bundle Adjustment strategy.") ("nbFirstUnstableCameras", po::value(&nbFirstUnstableCameras)->default_value(nbFirstUnstableCameras), "Number of cameras for which the bundle adjustment is performed every single time a camera is added, leading to more stable " @@ -138,7 +153,7 @@ int aliceVision_main(int argc, char** argv) ("bundleAdjustmentMaxOutliers", po::value(&bundleAdjustmentMaxOutliers)->default_value(bundleAdjustmentMaxOutliers), "Threshold for the maximum number of outliers allowed at the end of a bundle adjustment iteration." "Using a negative value for this threshold will disable BA iterations.") - ("weakResectionSize", po::value(&weakResectionSize)->default_value(weakResectionSize), + ("weakResectionSize", po::value(&weakResectionSize)->default_value(weakResectionSize), "When adding a view during the expansion process, we compute the pose. If the inliers count" "Is less than this value, the resection is considered weak. If not all views in the batch" "are weak, then the weak views are put back in the list of views to estimate again.") @@ -159,7 +174,7 @@ int aliceVision_main(int argc, char** argv) ("meshFilename,t", po::value(&meshFilename)->default_value(meshFilename), "Mesh file."); ; // clang-format on - + CmdLine cmdline("AliceVision SfM Expanding"); cmdline.add(requiredParams); @@ -173,7 +188,7 @@ int aliceVision_main(int argc, char** argv) HardwareContext hwc = cmdline.getHardwareContext(); hwc.setUserCoresLimit(100); omp_set_num_threads(hwc.getMaxThreads()); - + // load input SfMData scene sfmData::SfMData sfmData; if(!sfmDataIO::load(sfmData, sfmDataFilename, sfmDataIO::ESfMData::ALL)) @@ -222,14 +237,14 @@ int aliceVision_main(int argc, char** argv) sfm::ExpansionHistory::sptr expansionHistory = std::make_shared(); sfm::LbaPolicy::uptr sfmPolicy; - if (useLocalBA) + if (useLocalBA) { sfm::LbaPolicyConnexity::uptr sfmPolicyTyped = std::make_unique(); sfmPolicyTyped->setExpansionHistoryHandler(expansionHistory); sfmPolicyTyped->setDistanceLimit(lbaDistanceLimit); sfmPolicy = std::move(sfmPolicyTyped); } - + sfm::SfmBundle::uptr sfmBundle = std::make_unique(); sfmBundle->setLbaPolicyHandler(sfmPolicy); sfmBundle->setBundleAdjustmentMaxOutlier(bundleAdjustmentMaxOutliers); @@ -237,13 +252,14 @@ int aliceVision_main(int argc, char** argv) sfmBundle->setMaxReprojectionError(maxReprojectionError); sfmBundle->setMinNbCamerasToRefinePrincipalPoint(minNbCamerasToRefinePrincipalPoint); sfmBundle->setIsStructureRefinementEnabled(enableStructureRefinement); + sfmBundle->setTemporalConstraintParams(tempConstrParams, useTemporalConstraint, exitAfterPoseInterpolation); sfm::PointFetcher::uptr pointFetcherHandler; if (!meshFilename.empty()) { ALICEVISION_LOG_INFO("Load mesh"); std::unique_ptr handler = std::make_unique(); - + if (!handler->initialize(meshFilename)) { return EXIT_FAILURE; @@ -271,14 +287,14 @@ int aliceVision_main(int argc, char** argv) expansionChunk->setIgnoreMultiviewOnPrior(ignoreMultiviewOnPrior); expansionChunk->setMinAngleTriangulation(minAngleForTriangulation); expansionChunk->setWeakResectionSize(weakResectionSize); - + sfm::ExpansionPolicy::uptr expansionPolicy; { sfm::ExpansionPolicyLegacy::uptr expansionPolicyTyped = std::make_unique(); expansionPolicyTyped->setNbFirstUnstableViews(nbFirstUnstableCameras); expansionPolicyTyped->setMaxViewsPerGroup(maxImagesPerGroup); expansionPolicy = std::move(expansionPolicyTyped); - } + } sfm::ExpansionIteration::uptr expansionIteration = std::make_unique(); expansionIteration->setExpansionHistoryHandler(expansionHistory); @@ -308,8 +324,8 @@ int aliceVision_main(int argc, char** argv) sfmDataIO::save(sfmData, sfmDataOutputFilename, sfmDataIO::ESfMData::ALL); if (!outputSfMViewsAndPoses.empty()) - { - sfmDataIO::save(sfmData, outputSfMViewsAndPoses, + { + sfmDataIO::save(sfmData, outputSfMViewsAndPoses, sfmDataIO::ESfMData(sfmDataIO::VIEWS | sfmDataIO::EXTRINSICS | sfmDataIO::INTRINSICS) ); } diff --git a/src/software/utils/main_tracksSimulating.cpp b/src/software/utils/main_tracksSimulating.cpp index 4c41c99315..5c492004ba 100644 --- a/src/software/utils/main_tracksSimulating.cpp +++ b/src/software/utils/main_tracksSimulating.cpp @@ -36,6 +36,7 @@ int aliceVision_main(int argc, char** argv) double sigmaNoise = 0.0; double outlierRatio = 0.0; double outlierEpipolarRatio = 0.2; + bool randomNoiseVariancePerView = false; // clang-format off po::options_description requiredParams("Required parameters"); @@ -49,8 +50,9 @@ int aliceVision_main(int argc, char** argv) optionalParams.add_options() ("sigmaNoise", po::value(&sigmaNoise)->default_value(sigmaNoise)) ("outlierRatio", po::value(&outlierRatio)->default_value(outlierRatio)) - ("outlierEpipolarRatio", po::value(&outlierEpipolarRatio)->default_value(outlierEpipolarRatio)); - + ("outlierEpipolarRatio", po::value(&outlierEpipolarRatio)->default_value(outlierEpipolarRatio)) + ("randomNoiseVariancePerView", po::value(&randomNoiseVariancePerView)->default_value(randomNoiseVariancePerView), "Use different noise variance per view."); + // clang-format on CmdLine cmdline("AliceVision tracksSimulating"); @@ -78,7 +80,18 @@ int aliceVision_main(int argc, char** argv) std::uniform_real_distribution rand(0, 1); std::uniform_real_distribution randDepth(0.1, 100.0); std::normal_distribution randNoise(0.0, sigmaNoise); - + + std::map> viewRandNoise; + if (randomNoiseVariancePerView) + { + for (const auto viewId : sfmData.getViewsKeys()) + { + double viewSigma = std::abs(randNoise(generator)); + viewRandNoise.emplace(viewId, std::normal_distribution(0.0, viewSigma)); + ALICEVISION_LOG_INFO("View Random Noise Sigma : " << viewSigma); + } + } + track::TracksMap mapTracks; for (const auto & [landmarkId, landmark] : sfmData.getLandmarks()) @@ -105,7 +118,7 @@ int aliceVision_main(int argc, char** argv) Vec3 fakePoint = point * randDepth(generator) / point(2); obs = intrinsic.transformProject(pose.getTransform(), fakePoint.homogeneous(), true); } - else + else { std::uniform_real_distribution randWidth(0.0, intrinsic.w()); std::uniform_real_distribution randHeight(0.0, intrinsic.h()); @@ -114,19 +127,27 @@ int aliceVision_main(int argc, char** argv) obs.y() = randHeight(generator); } } - else + else { obs = intrinsic.transformProject(pose.getTransform(), point.homogeneous(), true); } - obs.x() += randNoise(generator); - obs.y() += randNoise(generator); + if (randomNoiseVariancePerView) + { + obs.x() += viewRandNoise.at(viewId)(generator); + obs.y() += viewRandNoise.at(viewId)(generator); + } + else + { + obs.x() += randNoise(generator); + obs.y() += randNoise(generator); + } track::TrackItem item; item.coords = obs; item.featureId = observation.getFeatureId(); item.scale = observation.getScale(); - + track.featPerView[viewId] = item; }