diff --git a/src/aliceVision/geometry/CMakeLists.txt b/src/aliceVision/geometry/CMakeLists.txt index 237be837f8..9a65eb7565 100644 --- a/src/aliceVision/geometry/CMakeLists.txt +++ b/src/aliceVision/geometry/CMakeLists.txt @@ -5,7 +5,6 @@ set(geometry_files_headers lie.hpp Pose3.hpp rigidTransformation3D.hpp - Similarity3.hpp Intersection.hpp ) diff --git a/src/aliceVision/geometry/Geometry.i b/src/aliceVision/geometry/Geometry.i index 3b2d8e392a..7a8277e738 100644 --- a/src/aliceVision/geometry/Geometry.i +++ b/src/aliceVision/geometry/Geometry.i @@ -10,5 +10,4 @@ %include %include %include -%include -%include \ No newline at end of file +%include \ No newline at end of file diff --git a/src/aliceVision/geometry/Similarity3.hpp b/src/aliceVision/geometry/Similarity3.hpp deleted file mode 100644 index 67c2872f30..0000000000 --- a/src/aliceVision/geometry/Similarity3.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2016 AliceVision contributors. -// Copyright (c) 2012 openMVG contributors. -// This Source Code Form is subject to the terms of the Mozilla Public License, -// v. 2.0. If a copy of the MPL was not distributed with this file, -// You can obtain one at https://mozilla.org/MPL/2.0/. - -#pragma once - -#include - -namespace aliceVision { -namespace geometry { - -// Define a 3D Similarity transform encoded as a 3D pose plus a scale -struct Similarity3 -{ - Pose3 _pose; - double _scale; - - Similarity3() - : _pose(Pose3()), - _scale(1.0) - {} - Similarity3(const Pose3& pose, const double scale) - : _pose(pose), - _scale(scale) - {} - - // Operators - Vec3 operator()(const Vec3& point) const { return _scale * _pose(point); } - - Pose3 operator()(const Pose3& pose) const { return Pose3(pose.rotation() * _pose.rotation().transpose(), this->operator()(pose.center())); } -}; - -} // namespace geometry -} // namespace aliceVision diff --git a/src/aliceVision/geometry/Similarity3.i b/src/aliceVision/geometry/Similarity3.i deleted file mode 100644 index 0f03e1d487..0000000000 --- a/src/aliceVision/geometry/Similarity3.i +++ /dev/null @@ -1,11 +0,0 @@ -// This file is part of the AliceVision project. -// Copyright (c) 2024 AliceVision contributors. -// This Source Code Form is subject to the terms of the Mozilla Public License, -// v. 2.0. If a copy of the MPL was not distributed with this file, -// You can obtain one at https://mozilla.org/MPL/2.0/. - -%include - -%{ -#include -%} \ No newline at end of file diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index 2a2e72dc87..d717b69c3a 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -30,135 +30,6 @@ namespace sfm { using namespace aliceVision::camera; using namespace aliceVision::geometry; -/** - * @brief Create the appropriate cost functor according the provided input camera intrinsic model - * @param[in] intrinsicPtr The intrinsic pointer - * @param[in] observation The corresponding observation - * @return cost functor - */ -ceres::CostFunction* createCostFunctionFromIntrinsics(const std::shared_ptr intrinsic, const sfmData::Observation& observation) -{ - auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ProjectionSimpleErrorFunctor(observation, intrinsic)); - - int distortionSize = 1; - auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic); - if (isod) - { - auto distortion = isod->getDistortion(); - if (distortion) - { - distortionSize = distortion->getParameters().size(); - } - } - - costFunction->AddParameterBlock(intrinsic->getParameters().size()); - costFunction->AddParameterBlock(distortionSize); - costFunction->AddParameterBlock(6); - costFunction->AddParameterBlock(3); - costFunction->SetNumResiduals(2); - - return costFunction; -} - -ceres::CostFunction* createSurveyPointCostFunction(const std::shared_ptr intrinsic, - const Vec3 & point, - const sfmData::Observation& observation) -{ - auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ProjectionSurveyErrorFunctor(point, observation, intrinsic)); - - int distortionSize = 1; - auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic); - if (isod) - { - auto distortion = isod->getDistortion(); - if (distortion) - { - distortionSize = distortion->getParameters().size(); - } - } - - costFunction->AddParameterBlock(intrinsic->getParameters().size()); - costFunction->AddParameterBlock(distortionSize); - costFunction->AddParameterBlock(6); - costFunction->SetNumResiduals(2); - - return costFunction; -} - -/** - * @brief Create the appropriate cost functor according the provided input rig camera intrinsic model - * @param[in] intrinsicPtr The intrinsic pointer - * @param[in] observation The corresponding observation - * @return cost functor - */ -ceres::CostFunction* createRigCostFunctionFromIntrinsics(std::shared_ptr intrinsic, const sfmData::Observation& observation) -{ - auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ProjectionErrorFunctor(observation, intrinsic)); - - int distortionSize = 1; - auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic); - if (isod) - { - auto distortion = isod->getDistortion(); - if (distortion) - { - distortionSize = distortion->getParameters().size(); - } - } - - costFunction->AddParameterBlock(intrinsic->getParameters().size()); - costFunction->AddParameterBlock(distortionSize); - costFunction->AddParameterBlock(6); - costFunction->AddParameterBlock(6); - costFunction->AddParameterBlock(3); - costFunction->SetNumResiduals(2); - - return costFunction; -} - -/** - * @brief Create the appropriate cost functor according the provided input camera intrinsic model - * @param[in] intrinsicPtr The intrinsic pointer - * @param[in] observation The corresponding observation - * @return cost functor - */ -ceres::CostFunction* createConstraintsCostFunctionFromIntrinsics(std::shared_ptr intrinsic, - const sfmData::Observation& observation_first, - const sfmData::Observation& observation_second) -{ - auto costFunction = new ceres::DynamicAutoDiffCostFunction(new Constraint2dErrorFunctor(observation_first, observation_second, intrinsic)); - - int distortionSize = 1; - auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic); - if (isod) - { - auto distortion = isod->getDistortion(); - if (distortion) - { - distortionSize = distortion->getParameters().size(); - } - } - - - costFunction->AddParameterBlock(intrinsic->getParameters().size()); - costFunction->AddParameterBlock(distortionSize); - costFunction->AddParameterBlock(6); - costFunction->AddParameterBlock(6); - costFunction->SetNumResiduals(2); - - return costFunction; -} - -ceres::CostFunction* createCostFunctionFromContraintPoint(const sfmData::Landmark & landmark, const Vec3 & normal) -{ - const double weight = 100.0; - auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ConstraintPointErrorFunctor(weight, normal, landmark.X)); - - costFunction->AddParameterBlock(3); - costFunction->SetNumResiduals(1); - - return costFunction; -} void BundleAdjustmentCeres::CeresOptions::setDenseBA() { @@ -664,7 +535,17 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat std::array& landmarkBlock = _landmarksBlocks[landmarkId]; for (std::size_t i = 0; i < 3; ++i) + { landmarkBlock.at(i) = landmark.X(Eigen::Index(i)); + } + + //If the landmark has a referenceViewIndex set, then retrieve the reference pose + double * referencePoseBlockPtr = nullptr; + if (landmark.referenceViewIndex != UndefinedIndexT) + { + const sfmData::View& refview = sfmData.getView(landmark.referenceViewIndex); + referencePoseBlockPtr = _posesBlocks.at(refview.getPoseId()).data(); + } double* landmarkBlockPtr = landmarkBlock.data(); problem.AddParameterBlock(landmarkBlockPtr, 3); @@ -707,7 +588,7 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat if (view.isPartOfRig() && !view.isPoseIndependant()) { - ceres::CostFunction* costFunction = createRigCostFunctionFromIntrinsics(intrinsic, observation); + ceres::CostFunction* costFunction = ProjectionErrorFunctor::createCostFunction(intrinsic, observation); double* rigBlockPtr = _rigBlocks.at(view.getRigId()).at(view.getSubPoseId()).data(); _linearSolverOrdering.AddElementToGroup(rigBlockPtr, 1); @@ -721,9 +602,26 @@ void BundleAdjustmentCeres::addLandmarksToProblem(const sfmData::SfMData& sfmDat problem.AddResidualBlock(costFunction, lossFunction, params); } + if (referencePoseBlockPtr != nullptr) + { + bool samePose = (referencePoseBlockPtr == poseBlockPtr); + ceres::CostFunction* costFunction = ProjectionRelativeErrorFunctor::createCostFunction(intrinsic, observation, samePose); + + std::vector params; + params.push_back(intrinsicBlockPtr); + params.push_back(distortionBlockPtr); + if (!samePose) + { + params.push_back(poseBlockPtr); + params.push_back(referencePoseBlockPtr); + } + params.push_back(landmarkBlockPtr); + + problem.AddResidualBlock(costFunction, lossFunction, params); + } else { - ceres::CostFunction* costFunction = createCostFunctionFromIntrinsics(intrinsic, observation); + ceres::CostFunction* costFunction = ProjectionSimpleErrorFunctor::createCostFunction(intrinsic, observation); std::vector params; params.push_back(intrinsicBlockPtr); @@ -787,8 +685,8 @@ void BundleAdjustmentCeres::addSurveyPointsToProblem(const sfmData::SfMData& sfm for (const auto & spoint: vspoints) { sfmData::Observation observation(spoint.survey, 0, 1.0); - ceres::CostFunction* costFunction = createSurveyPointCostFunction(intrinsic, spoint.point3d, observation); - + ceres::CostFunction* costFunction = + ProjectionSurveyErrorFunctor::createCostFunction(intrinsic, spoint.point3d, observation); std::vector params; params.push_back(intrinsicBlockPtr); @@ -843,7 +741,7 @@ void BundleAdjustmentCeres::addConstraints2DToProblem(const sfmData::SfMData& sf distortionBlockPtr_1 = _distortionsBlocks.at(intrinsicId_1).data(); } - ceres::CostFunction* costFunction = createConstraintsCostFunctionFromIntrinsics(intrinsicObject1, + ceres::CostFunction* costFunction = Constraint2dErrorFunctor::createCostFunction(intrinsicObject1, constraint.ObservationFirst, constraint.ObservationSecond); @@ -872,7 +770,7 @@ void BundleAdjustmentCeres::addConstraintsPointToProblem(const sfmData::SfMData& const sfmData::Landmark & l = sfmData.getLandmarks().at(landmarkId); double * ldata = _landmarksBlocks.at(landmarkId).data(); - ceres::CostFunction* costFunction = createCostFunctionFromContraintPoint(l, constraint.normal); + ceres::CostFunction* costFunction = ConstraintPointErrorFunctor::createCostFunction(l, constraint.normal); problem.AddResidualBlock(costFunction, lossFunction, ldata); } @@ -944,8 +842,9 @@ void BundleAdjustmentCeres::resetProblem() _intrinsicsBlocks.clear(); _landmarksBlocks.clear(); _rigBlocks.clear(); - - _linearSolverOrdering.Clear(); + _fakeDistortionBlock.clear(); + _intrinsicObjects.clear(); + _distortionsBlocks.clear(); } void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefineOptions refineOptions) const @@ -961,22 +860,10 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin if (refinePoses) { // absolute poses - for (auto& posePair : sfmData.getPoses()) + for (auto& [poseId, pose] : sfmData.getPoses()) { - const IndexT poseId = posePair.first; - - // do not update a camera pose set as Ignored or Constant in the Local strategy - if (posePair.second.getState() != EEstimatorParameterState::REFINED) - continue; - - const std::array& poseBlock = _posesBlocks.at(poseId); - - Mat3 R_refined; - ceres::AngleAxisToRotationMatrix(poseBlock.data(), R_refined.data()); - const Vec3 t_refined(poseBlock.at(3), poseBlock.at(4), poseBlock.at(5)); - - // update the pose - posePair.second.setTransform(poseFromRT(R_refined, t_refined)); + const std::array & poseBlock = _posesBlocks.at(poseId); + pose.updateFromExternal(poseBlock); } // rig sub-poses @@ -1002,11 +889,9 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin // update camera intrinsics with refined data if (refineIntrinsics) { - for (const auto& intrinsicBlockPair : _intrinsicsBlocks) + for (const auto& [idIntrinsic, block] : _intrinsicsBlocks) { - const IndexT intrinsicId = intrinsicBlockPair.first; - - const auto& intrinsic = sfmData.getIntrinsicSharedPtr(intrinsicId); + const auto& intrinsic = sfmData.getIntrinsicSharedPtr(idIntrinsic); // do not update a camera pose set as Ignored or Constant in the Local strategy if (intrinsic->getState() != EEstimatorParameterState::REFINED) @@ -1014,7 +899,7 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin continue; } - sfmData.getIntrinsics().at(intrinsicId)->updateFromParams(intrinsicBlockPair.second); + sfmData.getIntrinsics().at(idIntrinsic)->updateFromParams(block); } for (const auto& [idIntrinsic, distortionBlock]: _distortionsBlocks) @@ -1042,21 +927,10 @@ void BundleAdjustmentCeres::updateFromSolution(sfmData::SfMData& sfmData, ERefin // update landmarks if (refineStructure) { - for (const auto& landmarksBlockPair : _landmarksBlocks) + for (const auto& [idLandmark, block] : _landmarksBlocks) { - const IndexT landmarkId = landmarksBlockPair.first; - sfmData::Landmark& landmark = sfmData.getLandmarks().at(landmarkId); - - // do not update a camera pose set as Ignored or Constant in the Local strategy - if (landmark.state != EEstimatorParameterState::REFINED) - { - continue; - } - - for (std::size_t i = 0; i < 3; ++i) - { - landmark.X(Eigen::Index(i)) = landmarksBlockPair.second.at(i); - } + sfmData::Landmark& landmark = sfmData.getLandmarks().at(idLandmark); + landmark.updateFromExternal(block); } } } @@ -1150,6 +1024,11 @@ void BundleAdjustmentCeres::PrepareForEvaluation(bool evaluate_jacobians, bool n } } } + + for (const auto & [idLandmark, landmarkBlock]: _landmarksBlocks) + { + + } } } diff --git a/src/aliceVision/sfm/bundle/costfunctions/constraint2d.hpp b/src/aliceVision/sfm/bundle/costfunctions/constraint2d.hpp index 751238ea2e..7941f7bf4d 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/constraint2d.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/constraint2d.hpp @@ -54,6 +54,38 @@ struct Constraint2dErrorFunctor return _intrinsicProjectFunctor(projectParameters, residuals); } + /** + * @brief Create the appropriate cost functor according the provided input camera intrinsic model + * @param[in] intrinsicPtr The intrinsic pointer + * @param[in] observation The corresponding observation + * @return cost functor + */ + inline static ceres::CostFunction* createCostFunction(std::shared_ptr intrinsic, + const sfmData::Observation& observation_first, + const sfmData::Observation& observation_second) + { + auto costFunction = new ceres::DynamicAutoDiffCostFunction(new Constraint2dErrorFunctor(observation_first, observation_second, intrinsic)); + + int distortionSize = 1; + auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic); + if (isod) + { + auto distortion = isod->getDistortion(); + if (distortion) + { + distortionSize = distortion->getParameters().size(); + } + } + + costFunction->AddParameterBlock(intrinsic->getParameters().size()); + costFunction->AddParameterBlock(distortionSize); + costFunction->AddParameterBlock(6); + costFunction->AddParameterBlock(6); + costFunction->SetNumResiduals(2); + + return costFunction; + } + ceres::DynamicCostFunctionToFunctorTmp _intrinsicLiftFunctor; ceres::DynamicCostFunctionToFunctorTmp _intrinsicProjectFunctor; }; diff --git a/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp b/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp index f4ef19a2f7..303af0c0b7 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/constraintPoint.hpp @@ -30,6 +30,23 @@ struct ConstraintPointErrorFunctor return true; } + /** + * Create cost function for this class + * @param landmark the point coordinates + * @param normal the tangent surface to this point + * @return a cost function + */ + inline static ceres::CostFunction* createCostFunction(const sfmData::Landmark & landmark, const Vec3 & normal) + { + const double weight = 100.0; + auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ConstraintPointErrorFunctor(weight, normal, landmark.X)); + + costFunction->AddParameterBlock(3); + costFunction->SetNumResiduals(1); + + return costFunction; + } + double _weight; Vec3 _normal; double _constraintDistance; diff --git a/src/aliceVision/sfm/bundle/costfunctions/forceDepth.hpp b/src/aliceVision/sfm/bundle/costfunctions/forceDepth.hpp new file mode 100644 index 0000000000..e69a9ec026 --- /dev/null +++ b/src/aliceVision/sfm/bundle/costfunctions/forceDepth.hpp @@ -0,0 +1,43 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2025 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include + +struct ForceDepthErrorFunctor +{ + explicit ForceDepthErrorFunctor(double depth, double weight) + : pmean(mean) + , pweight(weight) + { + } + + template + bool operator()(T const* const* parameters, T* residuals) const + { + const T* parameter_point = parameters[0]; + + residuals[0] = pweight * (parameter_point[2] - T(pmean)); + + return true; + } + + inline static ceres::CostFunction* createCostFunction(double depth, double weight) + { + auto costFunction = new ceres::DynamicAutoDiffCostFunction( + new ForceDepthErrorFunctor(depth, weight)); + + costFunction->AddParameterBlock(3); + costFunction->SetNumResiduals(1); + + return costFunction; + } + + double pmean; + double pweight; +}; \ No newline at end of file diff --git a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp index 671844be12..6289906b78 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp @@ -58,6 +58,37 @@ struct ProjectionSimpleErrorFunctor return _intrinsicFunctor(innerParameters, residuals); } + /** + * @brief Create the appropriate cost functor according the provided input camera intrinsic model + * @param[in] intrinsicPtr The intrinsic pointer + * @param[in] observation The corresponding observation + * @return cost functor + */ + inline static ceres::CostFunction* createCostFunction( + const std::shared_ptr intrinsic, const sfmData::Observation& observation) + { + auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ProjectionSimpleErrorFunctor(observation, intrinsic)); + + int distortionSize = 1; + auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic); + if (isod) + { + auto distortion = isod->getDistortion(); + if (distortion) + { + distortionSize = distortion->getParameters().size(); + } + } + + costFunction->AddParameterBlock(intrinsic->getParameters().size()); + costFunction->AddParameterBlock(distortionSize); + costFunction->AddParameterBlock(6); + costFunction->AddParameterBlock(3); + costFunction->SetNumResiduals(2); + + return costFunction; + } + ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor; }; @@ -126,6 +157,39 @@ struct ProjectionSurveyErrorFunctor return true; } + /** + * @brief Create the appropriate cost functor according the provided input camera intrinsic model + * @param[in] intrinsicPtr The intrinsic pointer + * @param[in] point the reference point to compare to + * @param[in] observation The corresponding observation + * @return cost functor + */ + inline static ceres::CostFunction* createCostFunction( + const std::shared_ptr intrinsic, + const Vec3 & point, + const sfmData::Observation& observation) + { + auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ProjectionSurveyErrorFunctor(point, observation, intrinsic)); + + int distortionSize = 1; + auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic); + if (isod) + { + auto distortion = isod->getDistortion(); + if (distortion) + { + distortionSize = distortion->getParameters().size(); + } + } + + costFunction->AddParameterBlock(intrinsic->getParameters().size()); + costFunction->AddParameterBlock(distortionSize); + costFunction->AddParameterBlock(6); + costFunction->SetNumResiduals(2); + + return costFunction; + } + ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor; Vec3 _point; }; @@ -182,7 +246,160 @@ struct ProjectionErrorFunctor return _intrinsicFunctor(innerParameters, residuals); } + /** + * @brief Create the appropriate cost functor according the provided input rig camera intrinsic model + * @param[in] intrinsicPtr The intrinsic pointer + * @param[in] observation The corresponding observation + * @return cost functor + */ + inline static ceres::CostFunction* createCostFunction(std::shared_ptr intrinsic, + const sfmData::Observation& observation) + { + auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ProjectionErrorFunctor(observation, intrinsic)); + + int distortionSize = 1; + auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic); + if (isod) + { + auto distortion = isod->getDistortion(); + if (distortion) + { + distortionSize = distortion->getParameters().size(); + } + } + + costFunction->AddParameterBlock(intrinsic->getParameters().size()); + costFunction->AddParameterBlock(distortionSize); + costFunction->AddParameterBlock(6); + costFunction->AddParameterBlock(6); + costFunction->AddParameterBlock(3); + costFunction->SetNumResiduals(2); + + return costFunction; + } + + ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor; +}; + +struct ProjectionRelativeErrorFunctor +{ + explicit ProjectionRelativeErrorFunctor(const sfmData::Observation& obs + , const std::shared_ptr& intrinsics + , bool noPose) + : _intrinsicFunctor(new CostIntrinsicsProject(obs, intrinsics)) + , withoutPose(noPose) + { + } + + template + bool operator()(T const* const* parameters, T* residuals) const + { + const T* parameter_intrinsics = parameters[0]; + const T* parameter_distortion = parameters[1]; + + + T transformedPoint[3]; + if (withoutPose) + { + const T* parameter_relativepoint = parameters[2]; + transformedPoint[0] = parameter_relativepoint[0] / parameter_relativepoint[2]; + transformedPoint[1] = parameter_relativepoint[1] / parameter_relativepoint[2]; + transformedPoint[2] = 1.0 / parameter_relativepoint[2]; + } + else + { + const T* parameter_pose = parameters[2]; + const T* parameter_refpose = parameters[3]; + const T* parameter_relativepoint = parameters[4]; + + //From inverse depth to cartesian + T relpoint[3]; + relpoint[0] = parameter_relativepoint[0] / parameter_relativepoint[2]; + relpoint[1] = parameter_relativepoint[1] / parameter_relativepoint[2]; + relpoint[2] = 1.0 / parameter_relativepoint[2]; + + const T* refcam_R = parameter_refpose; + const T* refcam_t = ¶meter_refpose[3]; + + // Apply inverse reference transformation to cartesian landmark + relpoint[0] = relpoint[0] - refcam_t[0]; + relpoint[1] = relpoint[1] - refcam_t[1]; + relpoint[2] = relpoint[2] - refcam_t[2]; + + T invrefcam_R[3]; + invrefcam_R[0] = -refcam_R[0]; + invrefcam_R[1] = -refcam_R[1]; + invrefcam_R[2] = -refcam_R[2]; + + T absolutePoint[3]; + ceres::AngleAxisRotatePoint(invrefcam_R, relpoint, absolutePoint); + + //-- + // Apply external parameters (Pose) + //-- + const T* cam_R = parameter_pose; + const T* cam_t = ¶meter_pose[3]; + + + // Rotate the point according the camera rotation + ceres::AngleAxisRotatePoint(cam_R, absolutePoint, transformedPoint); + + // Apply the camera translation + transformedPoint[0] += cam_t[0]; + transformedPoint[1] += cam_t[1]; + transformedPoint[2] += cam_t[2]; + } + + const T * innerParameters[3]; + innerParameters[0] = parameter_intrinsics; + innerParameters[1] = parameter_distortion; + innerParameters[2] = transformedPoint; + + return _intrinsicFunctor(innerParameters, residuals); + } + + /** + * @brief Create the appropriate cost functor according the provided input camera intrinsic model + * @param[in] intrinsicPtr The intrinsic pointer + * @param[in] observation The corresponding observation + * @param[in] withoutPose When reference view and current view are the same, poses must be ignored + * @return cost functor + */ + inline static ceres::CostFunction* createCostFunction( + const std::shared_ptr intrinsic, + const sfmData::Observation& observation, + bool withoutPose) + { + auto costFunction = new ceres::DynamicAutoDiffCostFunction(new ProjectionRelativeErrorFunctor(observation, intrinsic, withoutPose)); + + int distortionSize = 1; + auto isod = camera::IntrinsicScaleOffsetDisto::cast(intrinsic); + if (isod) + { + auto distortion = isod->getDistortion(); + if (distortion) + { + distortionSize = distortion->getParameters().size(); + } + } + + costFunction->AddParameterBlock(intrinsic->getParameters().size()); + costFunction->AddParameterBlock(distortionSize); + + if (!withoutPose) + { + costFunction->AddParameterBlock(6); + costFunction->AddParameterBlock(6); + } + + costFunction->AddParameterBlock(3); + costFunction->SetNumResiduals(2); + + return costFunction; + } + ceres::DynamicCostFunctionToFunctorTmp _intrinsicFunctor; + bool withoutPose; }; diff --git a/src/aliceVision/sfmData/CameraPose.hpp b/src/aliceVision/sfmData/CameraPose.hpp index a19704f26c..1ca8ee8143 100644 --- a/src/aliceVision/sfmData/CameraPose.hpp +++ b/src/aliceVision/sfmData/CameraPose.hpp @@ -111,6 +111,23 @@ class CameraPose return _removable; } + inline void updateFromExternal(const std::array & data) + { + // do not update a camera pose set as Ignored or Constant in the Local strategy + if (getState() != EEstimatorParameterState::REFINED) + { + return; + } + + const Vec3 r_refined(data.at(0), data.at(1), data.at(2)); + const Vec3 t_refined(data.at(3), data.at(4), data.at(5)); + + const Mat3 R_refined = SO3::expm(r_refined); + + // update the pose + setTransform(geometry::poseFromRT(R_refined, t_refined)); + } + private: /// camera 3d transformation geometry::Pose3 _transform; diff --git a/src/aliceVision/sfmData/Landmark.hpp b/src/aliceVision/sfmData/Landmark.hpp index 58c608d7a0..771f4ea1e0 100644 --- a/src/aliceVision/sfmData/Landmark.hpp +++ b/src/aliceVision/sfmData/Landmark.hpp @@ -41,11 +41,15 @@ class Landmark feature::EImageDescriberType descType = feature::EImageDescriberType::UNINITIALIZED; image::RGBColor rgb = image::WHITE; //!> the color associated to the point EEstimatorParameterState state = EEstimatorParameterState::REFINED; + IndexT referenceViewIndex = UndefinedIndexT; 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 + && referenceViewIndex == other.referenceViewIndex; } inline bool operator!=(const Landmark& other) const { return !(*this == other); } @@ -54,6 +58,20 @@ class Landmark Observations& getObservations() { return _observations; } + inline void updateFromExternal(const std::array & data) + { + // do not update a camera pose set as Ignored or Constant in the Local strategy + if (state != EEstimatorParameterState::REFINED) + { + return; + } + + for (std::size_t i = 0; i < 3; ++i) + { + X(Eigen::Index(i)) = data.at(i); + } + } + private: Observations _observations; }; diff --git a/src/aliceVision/sfmDataIO/AlembicExporter.cpp b/src/aliceVision/sfmDataIO/AlembicExporter.cpp index ce01fa40c2..5153e17e86 100644 --- a/src/aliceVision/sfmDataIO/AlembicExporter.cpp +++ b/src/aliceVision/sfmDataIO/AlembicExporter.cpp @@ -505,11 +505,13 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks, return; // Fill vector with the values taken from AliceVision + std::vector referenceViewIndices; std::vector positions; std::vector colors; std::vector descTypes; positions.reserve(landmarks.size()); descTypes.reserve(landmarks.size()); + referenceViewIndices.reserve(landmarks.size()); // For all the 3d points in the hash_map for (const auto& landmark : landmarks) @@ -520,6 +522,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks, positions.emplace_back(pt[0], -pt[1], -pt[2]); colors.emplace_back(color.r() / 255.f, color.g() / 255.f, color.b() / 255.f); descTypes.emplace_back(static_cast(landmark.second.descType)); + referenceViewIndices.emplace_back(landmark.second.referenceViewIndex); } std::vector ids(positions.size()); @@ -542,6 +545,7 @@ void AlembicExporter::addLandmarks(const sfmData::Landmarks& landmarks, OCompoundProperty userProps = pSchema.getUserProperties(); OUInt32ArrayProperty(userProps, "mvg_describerType").set(descTypes); + OUInt32ArrayProperty(userProps, "mvg_referenceViewIndices").set(referenceViewIndices); if (withVisibility) { diff --git a/src/aliceVision/sfmDataIO/AlembicImporter.cpp b/src/aliceVision/sfmDataIO/AlembicImporter.cpp index eed1f66f9c..830f39091e 100644 --- a/src/aliceVision/sfmDataIO/AlembicImporter.cpp +++ b/src/aliceVision/sfmDataIO/AlembicImporter.cpp @@ -186,6 +186,18 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData:: } } + AV_UInt32ArraySamplePtr sampleReferenceViewIndices; + if (userProps && userProps.getPropertyHeader("mvg_referenceViewIndices")) + { + sampleReferenceViewIndices.read(userProps, "mvg_referenceViewIndices"); + if (sampleReferenceViewIndices.size() != positions->size()) + { + ALICEVISION_LOG_WARNING("[Alembic Importer] Describer type will be ignored. describerType vector size: " + << sampleReferenceViewIndices.size() << ", positions vector size: " << positions->size()); + sampleReferenceViewIndices.reset(); + } + } + // Number of points before adding the Alembic data const std::size_t nbPointsInit = sfmdata.getLandmarks().size(); for (std::size_t point3d_i = 0; point3d_i < positions->size(); ++point3d_i) @@ -217,6 +229,11 @@ bool readPointCloud(const Version& abcVersion, IObject iObj, M44d mat, sfmData:: const std::size_t descType_i = sampleDescs[point3d_i]; landmark.descType = static_cast(descType_i); } + + if (sampleReferenceViewIndices) + { + landmark.referenceViewIndex = sampleReferenceViewIndices[point3d_i]; + } } // for compatibility with files generated with a previous version diff --git a/src/aliceVision/sfmDataIO/jsonIO.cpp b/src/aliceVision/sfmDataIO/jsonIO.cpp index d0ea336ebe..9c71f1df98 100644 --- a/src/aliceVision/sfmDataIO/jsonIO.cpp +++ b/src/aliceVision/sfmDataIO/jsonIO.cpp @@ -500,6 +500,7 @@ void saveLandmark(const std::string& name, bpt::ptree landmarkTree; landmarkTree.put("landmarkId", landmarkId); + landmarkTree.put("referenceViewIndex", landmark.referenceViewIndex); landmarkTree.put("descType", feature::EImageDescriberType_enumToString(landmark.descType)); saveMatrix("color", landmark.rgb, landmarkTree); @@ -538,7 +539,7 @@ void loadLandmark(IndexT& landmarkId, sfmData::Landmark& landmark, bpt::ptree& l { landmarkId = landmarkTree.get("landmarkId"); landmark.descType = feature::EImageDescriberType_stringToEnum(landmarkTree.get("descType")); - + landmark.referenceViewIndex = landmarkTree.get("referenceViewIndex", UndefinedIndexT); loadMatrix("color", landmark.rgb, landmarkTree); loadMatrix("X", landmark.X, landmarkTree);