diff --git a/src/aliceVision/multiview/NViewDataSet.hpp b/src/aliceVision/multiview/NViewDataSet.hpp index ebd65d4610..2f24557b5b 100644 --- a/src/aliceVision/multiview/NViewDataSet.hpp +++ b/src/aliceVision/multiview/NViewDataSet.hpp @@ -61,6 +61,8 @@ struct NViewDatasetConfigurator double _jitter_amount; NViewDatasetConfigurator(int fx = 1000, int fy = 1000, int cx = 500, int cy = 500, double distance = 1.5, double jitter_amount = 0.01); + + bool _useRelative = false; }; /** diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index 9b83fd08fc..d7e47ac1db 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -533,7 +533,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); @@ -590,6 +600,23 @@ 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 = ProjectionSimpleErrorFunctor::createCostFunction(intrinsic, observation); diff --git a/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp b/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp index ccf4b84b1c..c666c6c1ad 100644 --- a/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp +++ b/src/aliceVision/sfm/bundle/bundleAdjustment_test.cpp @@ -54,6 +54,27 @@ BOOST_AUTO_TEST_CASE(BUNDLE_ADJUSTMENT_EffectiveMinimization_Pinhole) BOOST_CHECK_LT(dResidual_after, dResidual_before); } +BOOST_AUTO_TEST_CASE(BUNDLE_ADJUSTMENT_EffectiveMinimization_Relative_Pinhole) +{ + const int nviews = 3; + const int npoints = 6; + NViewDatasetConfigurator config; + config._useRelative = true; + const NViewDataSet d = NRealisticCamerasRing(nviews, npoints, config); + + // Translate the input dataset to a SfMData scene + SfMData sfmData = getInputScene(d, config, EINTRINSIC::PINHOLE_CAMERA, EDISTORTION::DISTORTION_NONE); + + const double dResidual_before = RMSE(sfmData); + + // Call the BA interface and let it refine (Structure and Camera parameters [Intrinsics|Motion]) + std::shared_ptr ba_object = std::make_shared(); + BOOST_CHECK(ba_object->adjust(sfmData)); + + const double dResidual_after = RMSE(sfmData); + BOOST_CHECK_LT(dResidual_after, dResidual_before); +} + BOOST_AUTO_TEST_CASE(BUNDLE_ADJUSTMENT_EffectiveMinimization_PinholeRadialK1) { const int nviews = 3; @@ -308,6 +329,15 @@ SfMData getInputScene(const NViewDataSet& d, const NViewDatasetConfigurator& con // Collect the image of point i in each frame. Landmark landmark; landmark.X = d._X.col(i); + + if (config._useRelative) + { + landmark.referenceViewIndex = nviews / 2; + geometry::Pose3 p = sfm_data.getAbsolutePose(landmark.referenceViewIndex).getTransform(); + geometry::Pose3 pinv = p.inverse(); + landmark.X = pinv(landmark.X); + } + for (int j = 0; j < nviews; ++j) { Vec2 pt = d._x[j].col(i); @@ -317,6 +347,7 @@ SfMData getInputScene(const NViewDataSet& d, const NViewDatasetConfigurator& con landmark.getObservations()[j] = Observation(pt, i, unknownScale); } + sfm_data.getLandmarks()[i] = landmark; } diff --git a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp index 972de7ebb1..01936bf624 100644 --- a/src/aliceVision/sfm/bundle/costfunctions/projection.hpp +++ b/src/aliceVision/sfm/bundle/costfunctions/projection.hpp @@ -281,6 +281,130 @@ struct ProjectionErrorFunctor 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) + { + //withoutPose is used if reference view and current view are the same views + const T* parameter_relativepoint = parameters[2]; + transformedPoint[0] = parameter_relativepoint[0]; + transformedPoint[1] = parameter_relativepoint[1]; + transformedPoint[2] = parameter_relativepoint[2]; + } + else + { + const T* parameter_pose = parameters[2]; + const T* parameter_refpose = parameters[3]; + const T* parameter_relativepoint = parameters[4]; + + //Retrieve point + T relpoint[3]; + relpoint[0] = parameter_relativepoint[0]; + relpoint[1] = parameter_relativepoint[1]; + relpoint[2] = parameter_relativepoint[2]; + + const T* refcam_R = parameter_refpose; + const T* refcam_t = ¶meter_refpose[3]; + + // Apply transformation such that the point + // Which was defined in the camera geometric frame + // is now defined in the world frame + 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; +}; + } // namespace sfm } // namespace aliceVision diff --git a/src/aliceVision/sfmData/Landmark.hpp b/src/aliceVision/sfmData/Landmark.hpp index 37798fa173..500bfe4377 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); } diff --git a/src/aliceVision/sfmDataIO/AlembicExporter.cpp b/src/aliceVision/sfmDataIO/AlembicExporter.cpp index f34e4f69f5..157bc24155 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 b918ddb4e5..d2301c2f9f 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 0ff5ece516..6f4fdf66b7 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);