diff --git a/src/aliceVision/sfm/pipeline/relativePoses.hpp b/src/aliceVision/sfm/pipeline/relativePoses.hpp new file mode 100644 index 0000000000..5dfc677ae8 --- /dev/null +++ b/src/aliceVision/sfm/pipeline/relativePoses.hpp @@ -0,0 +1,92 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include +#include +#include +#include + +namespace Eigen +{ + template + Eigen::Matrix tag_invoke(boost::json::value_to_tag>, boost::json::value const& jv) + { + Eigen::Matrix ret; + + std::vector buf = boost::json::value_to>(jv); + + int pos = 0; + for (int i = 0; i < M; i ++) + { + for (int j = 0; j < N; j++) + { + ret(i, j) = buf[pos]; + pos++; + } + } + + return ret; + } + + template + void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, Eigen::Matrix const& input) + { + std::vector buf; + + for (int i = 0; i < M; i ++) + { + for (int j = 0; j < N; j++) + { + buf.push_back(input(i, j)); + } + } + + + jv = boost::json::value_from>(std::move(buf)); + } + +} + +namespace aliceVision +{ +namespace sfm +{ + +struct ReconstructedPair +{ + IndexT reference; + IndexT next; + Mat3 R; + Vec3 t; +}; + + +void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, sfm::ReconstructedPair const& input) +{ + jv = { + {"reference", input.reference}, + {"next", input.next}, + {"R", boost::json::value_from(SO3::logm(input.R))}, + {"t", boost::json::value_from(input.t)} + }; +} + +ReconstructedPair tag_invoke(boost::json::value_to_tag, boost::json::value const& jv) +{ + const boost::json::object& obj = jv.as_object(); + + ReconstructedPair ret; + + ret.reference = boost::json::value_to(obj.at("reference")); + ret.next = boost::json::value_to(obj.at("next")); + ret.R = SO3::expm(boost::json::value_to(obj.at("R"))); + ret.t = boost::json::value_to(obj.at("t")); + + return ret; +} + +} +} \ No newline at end of file diff --git a/src/aliceVision/track/trackIO.cpp b/src/aliceVision/track/trackIO.cpp index 31d69858bd..c99c707737 100644 --- a/src/aliceVision/track/trackIO.cpp +++ b/src/aliceVision/track/trackIO.cpp @@ -17,5 +17,16 @@ void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, alic }; } +aliceVision::track::Track tag_invoke(boost::json::value_to_tag, boost::json::value const& jv) +{ + const boost::json::object& obj = jv.as_object(); + + aliceVision::track::Track ret; + ret.descType = feature::EImageDescriberType_stringToEnum(boost::json::value_to(obj.at("descType"))); + ret.featPerView = flat_map_value_to(obj.at("featPerView")); + + return ret; +} + } // namespace track } // namespace aliceVision diff --git a/src/aliceVision/track/trackIO.hpp b/src/aliceVision/track/trackIO.hpp index 4c351750b0..da9d3f67ec 100644 --- a/src/aliceVision/track/trackIO.hpp +++ b/src/aliceVision/track/trackIO.hpp @@ -13,10 +13,31 @@ namespace aliceVision { namespace track { +template +stl::flat_map flat_map_value_to(const boost::json::value& jv) +{ + stl::flat_map ret; + + const boost::json::array obj = jv.as_array(); + + for (const auto & item: obj) + { + const boost::json::array inner = item.as_array(); + ret.insert({boost::json::value_to(inner[0]), boost::json::value_to(inner[1])}); + } + + return ret; +} + /** * @brief Serialize track to JSON object. */ void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, aliceVision::track::Track const& input); +/** + * @brief Deserialize track from JSON object. + */ +aliceVision::track::Track tag_invoke(boost::json::value_to_tag, boost::json::value const& jv); + } // namespace track } // namespace aliceVision diff --git a/src/software/pipeline/CMakeLists.txt b/src/software/pipeline/CMakeLists.txt index 9dde200758..fb9ca27b98 100644 --- a/src/software/pipeline/CMakeLists.txt +++ b/src/software/pipeline/CMakeLists.txt @@ -125,6 +125,36 @@ if(ALICEVISION_BUILD_SFM) Boost::json ) + # Relative pose + alicevision_add_software(aliceVision_relativePoseEstimating + SOURCE main_relativePoseEstimating.cpp + FOLDER ${FOLDER_SOFTWARE_PIPELINE} + LINKS aliceVision_system + aliceVision_cmdline + aliceVision_feature + aliceVision_sfm + aliceVision_sfmData + aliceVision_track + Boost::program_options + Boost::filesystem + Boost::json + ) + + # bootstraping sfm + alicevision_add_software(aliceVision_sfmBootstraping + SOURCE main_sfmBootstraping.cpp + FOLDER ${FOLDER_SOFTWARE_PIPELINE} + LINKS aliceVision_system + aliceVision_cmdline + aliceVision_feature + aliceVision_sfm + aliceVision_sfmData + aliceVision_track + Boost::program_options + Boost::filesystem + Boost::json + ) + # Incremental / Sequential SfM alicevision_add_software(aliceVision_incrementalSfM SOURCE main_incrementalSfM.cpp diff --git a/src/software/pipeline/main_relativePoseEstimating.cpp b/src/software/pipeline/main_relativePoseEstimating.cpp new file mode 100644 index 0000000000..1b0a1277fd --- /dev/null +++ b/src/software/pipeline/main_relativePoseEstimating.cpp @@ -0,0 +1,402 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include + +// These constants define the current software version. +// They must be updated when the command line is changed. +#define ALICEVISION_SOFTWARE_VERSION_MAJOR 1 +#define ALICEVISION_SOFTWARE_VERSION_MINOR 0 + +using namespace aliceVision; + +namespace po = boost::program_options; +namespace fs = boost::filesystem; + + +bool getPoseStructure(Mat3& R, Vec3& t, std::vector& structure, std::vector& newVecInliers, const Mat3& E, + const std::vector& vecInliers, const Mat3& K1, const Mat3& K2, const Mat& x1, + const Mat& x2) +{ + // Find set of analytical solutions + std::vector Rs; + std::vector ts; + motionFromEssential(E, &Rs, &ts); + + Mat34 P1, P2; + Mat3 R1 = Mat3::Identity(); + Vec3 t1 = Vec3::Zero(); + P_from_KRt(K1, R1, t1, &P1); + + size_t bestCoundValid = 0; + + for(int it = 0; it < Rs.size(); it++) + { + const Mat3& R2 = Rs[it]; + const Vec3& t2 = ts[it]; + + P_from_KRt(K2, R2, t2, &P2); + + std::vector points; + std::vector updatedInliers; + + size_t countValid = 0; + for(size_t k = 0; k < vecInliers.size(); ++k) + { + const Vec2& pt1 = x1.col(vecInliers[k]); + const Vec2& pt2 = x2.col(vecInliers[k]); + + Vec3 X; + multiview::TriangulateDLT(P1, pt1, P2, pt2, &X); + + // Test if point is front to the two cameras. + if(Depth(R1, t1, X) > 0 && Depth(R2, t2, X) > 0) + { + countValid++; + } + + updatedInliers.push_back(vecInliers[k]); + points.push_back(X); + } + + if(countValid > bestCoundValid) + { + bestCoundValid = countValid; + structure = points; + newVecInliers = updatedInliers; + R = Rs[it]; + t = ts[it]; + } + } + + if(newVecInliers.size() < 10) + { + return false; + } + + return true; +} + +bool robustEssential(Mat3& E, std::vector& vecInliers, const Mat3& K1, const Mat3& K2, const Mat& x1, + const Mat& x2, const std::pair& size_ima1, + const std::pair& size_ima2, std::mt19937& randomNumberGenerator, + const size_t maxIterationCount, const size_t minInliers) +{ + // use the 5 point solver to estimate E + using SolverT = multiview::relativePose::Essential5PSolver; + + // define the kernel + using KernelT = + multiview::RelativePoseKernel_K; + + KernelT kernel(x1, size_ima1.first, size_ima1.second, x2, size_ima2.first, size_ima2.second, K1, K2); + + robustEstimation::Mat3Model model; + vecInliers.clear(); + + // robustly estimation of the Essential matrix and its precision + const std::pair acRansacOut = + robustEstimation::ACRANSAC(kernel, randomNumberGenerator, vecInliers, maxIterationCount, &model, Square(4.0)); + + if(vecInliers.size() < minInliers) + { + return false; + } + + E = model.getMatrix(); + + return true; +} + +void computeCovisibility(std::map& covisibility, const track::TracksMap& mapTracks) +{ + for(const auto& item : mapTracks) + { + const auto& track = item.second; + + for(auto it = track.featPerView.begin(); it != track.featPerView.end(); it++) + { + Pair p; + p.first = it->first; + + for(auto next = std::next(it); next != track.featPerView.end(); next++) + { + p.second = next->first; + + if(covisibility.find(p) == covisibility.end()) + { + covisibility[p] = 0; + } + else + { + covisibility[p]++; + } + } + } + } +} + +int aliceVision_main(int argc, char** argv) +{ + // command-line parameters + std::string sfmDataFilename; + std::vector featuresFolders; + std::string tracksFilename; + std::string outputDirectory; + int rangeStart = -1; + int rangeSize = 1; + const size_t minInliers = 35; + + // user optional parameters + std::string describerTypesName = feature::EImageDescriberType_enumToString(feature::EImageDescriberType::SIFT); + + + int randomSeed = std::mt19937::default_seed; + + po::options_description requiredParams("Required parameters"); + requiredParams.add_options() + ("input,i", po::value(&sfmDataFilename)->required(), "SfMData file.") + ("tracksFilename,i", po::value(&tracksFilename)->required(), "Tracks file.") + ("output,o", po::value(&outputDirectory)->required(), "Path to the output directory."); + + po::options_description optionalParams("Optional parameters"); + optionalParams.add_options() + ("featuresFolders,f", po::value>(&featuresFolders)->multitoken(), "Path to folder(s) containing the extracted features.") + ("describerTypes,d", po::value(&describerTypesName)->default_value(describerTypesName),feature::EImageDescriberType_informations().c_str()) + ("rangeStart", po::value(&rangeStart)->default_value(rangeStart), "Range image index start.") + ("rangeSize", po::value(&rangeSize)->default_value(rangeSize), "Range size."); + + CmdLine cmdline("AliceVision pairsEstimations"); + + cmdline.add(requiredParams); + cmdline.add(optionalParams); + if(!cmdline.execute(argc, argv)) + { + return EXIT_FAILURE; + } + + // set maxThreads + HardwareContext hwc = cmdline.getHardwareContext(); + omp_set_num_threads(hwc.getMaxThreads()); + + std::mt19937 randomNumberGenerator(randomSeed); + + // load input SfMData scene + sfmData::SfMData sfmData; + if(!sfmDataIO::Load(sfmData, sfmDataFilename, sfmDataIO::ESfMData::ALL)) + { + ALICEVISION_LOG_ERROR("The input SfMData file '" + sfmDataFilename + "' cannot be read."); + return EXIT_FAILURE; + } + + // Define range to compute + if(rangeStart != -1) + { + if(rangeStart < 0 || rangeSize < 0 || rangeStart > sfmData.getViews().size()) + { + ALICEVISION_LOG_ERROR("Range is incorrect"); + return EXIT_FAILURE; + } + + if(rangeStart + rangeSize > sfmData.getViews().size()) + { + rangeSize = sfmData.getViews().size() - rangeStart; + } + } + else + { + rangeStart = 0; + rangeSize = sfmData.getViews().size(); + } + ALICEVISION_LOG_DEBUG("Range to compute: rangeStart=" << rangeStart << ", rangeSize=" << rangeSize); + + + + // get imageDescriber type + const std::vector describerTypes = + feature::EImageDescriberType_stringToEnums(describerTypesName); + + + // features reading + feature::FeaturesPerView featuresPerView; + ALICEVISION_LOG_INFO("Load features"); + if(!sfm::loadFeaturesPerView(featuresPerView, sfmData, featuresFolders, describerTypes)) + { + ALICEVISION_LOG_ERROR("Invalid features."); + return EXIT_FAILURE; + } + + // Load tracks + ALICEVISION_LOG_INFO("Load tracks"); + std::ifstream tracksFile(tracksFilename); + if(tracksFile.is_open() == false) + { + ALICEVISION_LOG_ERROR("The input tracks file '" + tracksFilename + "' cannot be read."); + return EXIT_FAILURE; + } + std::stringstream buffer; + buffer << tracksFile.rdbuf(); + boost::json::value jv = boost::json::parse(buffer.str()); + track::TracksMap mapTracks(track::flat_map_value_to(jv)); + + // Compute tracks per view + ALICEVISION_LOG_INFO("Estimate tracks per view"); + track::TracksPerView mapTracksPerView; + for(const auto& viewIt : sfmData.views) + { + // create an entry in the map + mapTracksPerView[viewIt.first]; + } + track::computeTracksPerView(mapTracks, mapTracksPerView); + + ALICEVISION_LOG_INFO("Compute co-visibility"); + std::map covisibility; + computeCovisibility(covisibility, mapTracks); + + + + ALICEVISION_LOG_INFO("Process co-visibility"); + std::stringstream ss; + ss << outputDirectory << "/pairs_" << rangeStart << ".json"; + std::ofstream of(ss.str()); + + std::vector reconstructedPairs; + + double ratioChunk = double(covisibility.size()) / double(sfmData.getViews().size()); + int chunkStart = int(double(rangeStart) * ratioChunk); + int chunkEnd = int(double(rangeStart + rangeSize) * ratioChunk); + + //For each covisible pair +#pragma omp parallel for + for(int posPairs = chunkStart; posPairs < chunkEnd; posPairs++) + { + auto iterPairs = covisibility.begin(); + std::advance(iterPairs, posPairs); + + //Retrieve pair information + IndexT refImage = iterPairs->first.first; + IndexT nextImage = iterPairs->first.second; + + const sfmData::View& refView = sfmData.getView(refImage); + const sfmData::View& nextView = sfmData.getView(nextImage); + + std::shared_ptr refIntrinsics = sfmData.getIntrinsicsharedPtr(refView.getIntrinsicId()); + std::shared_ptr nextIntrinsics = + sfmData.getIntrinsicsharedPtr(nextView.getIntrinsicId()); + std::shared_ptr refPinhole = std::dynamic_pointer_cast(refIntrinsics); + std::shared_ptr nextPinhole = std::dynamic_pointer_cast(nextIntrinsics); + + aliceVision::track::TracksMap mapTracksCommon; + track::getCommonTracksInImagesFast({refImage, nextImage}, mapTracks, mapTracksPerView, mapTracksCommon); + + feature::MapFeaturesPerDesc& refFeaturesPerDesc = featuresPerView.getFeaturesPerDesc(refImage); + feature::MapFeaturesPerDesc& nextFeaturesPerDesc = featuresPerView.getFeaturesPerDesc(nextImage); + + //Build features coordinates matrices + const std::size_t n = mapTracksCommon.size(); + Mat refX(2, n); + Mat nextX(2, n); + IndexT pos = 0; + for(const auto& commonItem : mapTracksCommon) + { + const track::Track& track = commonItem.second; + + const feature::PointFeatures& refFeatures = refFeaturesPerDesc.at(track.descType); + const feature::PointFeatures& nextfeatures = nextFeaturesPerDesc.at(track.descType); + + IndexT refFeatureId = track.featPerView.at(refImage); + IndexT nextfeatureId = track.featPerView.at(nextImage); + + refX.col(pos) = refFeatures[refFeatureId].coords().cast(); + nextX.col(pos) = nextfeatures[nextfeatureId].coords().cast(); + + pos++; + } + + + //Try to fit an essential matrix (we assume we are approx. calibrated) + Mat3 E; + std::vector vec_inliers; + const bool essentialSuccess = + robustEssential(E, vec_inliers, refPinhole->K(), nextPinhole->K(), refX, nextX, + std::make_pair(refPinhole->w(), refPinhole->h()), + std::make_pair(nextPinhole->w(), nextPinhole->h()), + randomNumberGenerator, 1024, minInliers); + if(!essentialSuccess) + { + continue; + } + + std::vector structure; + std::vector inliers; + sfm::ReconstructedPair reconstructed; + reconstructed.reference = refImage; + reconstructed.next = nextImage; + + if(!getPoseStructure(reconstructed.R, reconstructed.t, structure, inliers, E, vec_inliers, refPinhole->K(), + nextPinhole->K(), refX, nextX)) + { + continue; + } + + //Buffered output to avoid lo +#pragma omp critical + { + reconstructedPairs.push_back(reconstructed); + + if(reconstructedPairs.size() > 1000) + { + boost::json::value jv = boost::json::value_from(reconstructedPairs); + of << boost::json::serialize(jv); + reconstructedPairs.clear(); + } + } + } + + //Serialize last pairs + { + boost::json::value jv = boost::json::value_from(reconstructedPairs); + of << boost::json::serialize(jv); + } + + of.close(); + + return EXIT_SUCCESS; +} diff --git a/src/software/pipeline/main_sfmBootstraping.cpp b/src/software/pipeline/main_sfmBootstraping.cpp new file mode 100644 index 0000000000..bc40cc37ec --- /dev/null +++ b/src/software/pipeline/main_sfmBootstraping.cpp @@ -0,0 +1,447 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2023 AliceVision contributors. +// This Source Code Form is subject to the terms of the Mozilla Public License, +// v. 2.0. If a copy of the MPL was not distributed with this file, +// You can obtain one at https://mozilla.org/MPL/2.0/. + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +// These constants define the current software version. +// They must be updated when the command line is changed. +#define ALICEVISION_SOFTWARE_VERSION_MAJOR 1 +#define ALICEVISION_SOFTWARE_VERSION_MINOR 0 + +using namespace aliceVision; + +namespace po = boost::program_options; +namespace fs = boost::filesystem; + +std::vector readJsons(std::istream& is, boost::json::error_code& ec) +{ + std::vector jvs; + boost::json::stream_parser p; + std::string line; + std::size_t n = 0; + + + while(true) + { + if(n == line.size()) + { + if(!std::getline(is, line)) + { + break; + } + + n = 0; + } + + //Consume at least part of the line + n += p.write_some( line.data() + n, line.size() - n, ec); + + //If the parser found a value, add it + if (p.done()) + { + jvs.push_back(p.release()); + p.reset(); + } + } + + if (!p.done()) + { + //Try to extract the end + p.finish(ec); + if (ec.failed()) + { + return jvs; + } + + jvs.push_back(p.release()); + } + + return jvs; +} + +bool estimatePairAngle(const sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pair, const track::TracksMap& tracksMap, const track::TracksPerView & tracksPerView, const feature::FeaturesPerView & featuresPerView, const double maxDistance, double & resultAngle, std::vector & usedTracks) +{ + usedTracks.clear(); + + const sfmData::View& refView = sfmData.getView(pair.reference); + const sfmData::View& nextView = sfmData.getView(pair.next); + + std::shared_ptr refIntrinsics = sfmData.getIntrinsicsharedPtr(refView.getIntrinsicId()); + std::shared_ptr nextIntrinsics = sfmData.getIntrinsicsharedPtr(nextView.getIntrinsicId()); + std::shared_ptr refPinhole = std::dynamic_pointer_cast(refIntrinsics); + std::shared_ptr nextPinhole = std::dynamic_pointer_cast(nextIntrinsics); + + if (refPinhole==nullptr || nextPinhole == nullptr) + { + return false; + } + + aliceVision::track::TracksMap mapTracksCommon; + track::getCommonTracksInImagesFast({pair.reference, pair.next}, tracksMap, tracksPerView, mapTracksCommon); + + const feature::MapFeaturesPerDesc& refFeaturesPerDesc = featuresPerView.getFeaturesPerDesc(pair.reference); + const feature::MapFeaturesPerDesc& nextFeaturesPerDesc = featuresPerView.getFeaturesPerDesc(pair.next); + + const Eigen::Matrix3d Kref = refPinhole->K(); + const Eigen::Matrix3d Knext = nextPinhole->K(); + + Eigen::Matrix3d F = Knext.inverse().transpose() * CrossProductMatrix(pair.t) * pair.R * Kref.inverse(); + + Eigen::Vector3d c = - pair.R.transpose() * pair.t; + + Mat34 P1, P2; + P_from_KRt(Kref, Mat3::Identity(), Vec3::Zero(), &P1); + P_from_KRt(Knext, pair.R, pair.t, &P2); + + size_t count = 0; + std::vector angles; + for(const auto& commonItem : mapTracksCommon) + { + const track::Track& track = commonItem.second; + const feature::PointFeatures& refFeatures = refFeaturesPerDesc.at(track.descType); + const feature::PointFeatures& nextfeatures = nextFeaturesPerDesc.at(track.descType); + const IndexT refFeatureId = track.featPerView.at(pair.reference); + const IndexT nextfeatureId = track.featPerView.at(pair.next); + const Vec2 refpt = refFeatures[refFeatureId].coords().cast(); + const Vec2 nextpt = nextfeatures[nextfeatureId].coords().cast(); + + const Vec2 refptu = refIntrinsics->get_ud_pixel(refpt); + const Vec2 nextptu = nextIntrinsics->get_ud_pixel(nextpt); + + Vec3 line = F * refptu.homogeneous(); + + //Make sure line normal is normalized + line = line * (1.0 / line.head<2>().norm()); + double distance = nextptu.homogeneous().dot(line); + if (distance > maxDistance) + { + continue; + } + + Vec3 X; + multiview::TriangulateDLT(P1, refptu, P2, nextptu, &X); + + if (X(2) < 0.0) + { + continue; + } + + const Vec3 ray1 = - X; + const Vec3 ray2 = c - X; + const double cangle = clamp(ray1.normalized().dot(ray2.normalized()), -1.0, 1.0); + const double angle = std::acos(cangle); + angles.push_back(angle); + + usedTracks.push_back(commonItem.first); + } + + const unsigned medianIndex = angles.size() / 2; + std::nth_element(angles.begin(), angles.begin() + medianIndex, angles.end()); + resultAngle = angles[medianIndex]; + + return true; +} + +double computeScore(const feature::FeaturesPerView & featuresPerView, const track::TracksMap & tracksMap, const std::vector & usedTracks, const IndexT viewId, const size_t maxLevel) +{ + const feature::MapFeaturesPerDesc& featuresPerDesc = featuresPerView.getFeaturesPerDesc(viewId); + + std::vector>> uniques(maxLevel - 1); + + for (auto trackId : usedTracks) + { + auto & track = tracksMap.at(trackId); + + const feature::PointFeatures& features = featuresPerDesc.at(track.descType); + + const IndexT featureId = track.featPerView.at(viewId); + const Vec2 pt = features[featureId].coords().cast(); + + unsigned int ptx = (unsigned int)(pt.x()); + unsigned int pty = (unsigned int)(pt.y()); + + for (unsigned int shift = 1; shift < maxLevel; shift++) + { + unsigned int lptx = ptx >> shift; + unsigned int lpty = pty >> shift; + + uniques[shift - 1].insert(std::make_pair(lptx, lpty)); + } + } + + double sum = 0.0; + for (unsigned int shift = 1; shift < maxLevel; shift++) + { + int size = uniques[shift - 1].size(); + if (size <= 1) + { + continue; + } + + double w = pow(2.0, maxLevel - shift); + sum += w * double(size); + } + + return sum; +} + + +bool buildSfmData(sfmData::SfMData & sfmData, const sfm::ReconstructedPair & pair, const track::TracksMap& tracksMap, const feature::FeaturesPerView & featuresPerView, const std::vector & usedTracks) +{ + const sfmData::View& refView = sfmData.getView(pair.reference); + const sfmData::View& nextView = sfmData.getView(pair.next); + + std::shared_ptr refIntrinsics = sfmData.getIntrinsicsharedPtr(refView.getIntrinsicId()); + std::shared_ptr nextIntrinsics = sfmData.getIntrinsicsharedPtr(nextView.getIntrinsicId()); + std::shared_ptr refPinhole = std::dynamic_pointer_cast(refIntrinsics); + std::shared_ptr nextPinhole = std::dynamic_pointer_cast(nextIntrinsics); + + if (refPinhole==nullptr || nextPinhole == nullptr) + { + return false; + } + + const feature::MapFeaturesPerDesc& refFeaturesPerDesc = featuresPerView.getFeaturesPerDesc(pair.reference); + const feature::MapFeaturesPerDesc& nextFeaturesPerDesc = featuresPerView.getFeaturesPerDesc(pair.next); + + const Eigen::Matrix3d Kref = refPinhole->K(); + const Eigen::Matrix3d Knext = nextPinhole->K(); + + Mat34 P1, P2; + P_from_KRt(Kref, Mat3::Identity(), Vec3::Zero(), &P1); + P_from_KRt(Knext, pair.R, pair.t, &P2); + + size_t count = 0; + std::vector angles; + for(const auto& trackId : usedTracks) + { + const track::Track& track = tracksMap.at(trackId); + + const feature::PointFeatures& refFeatures = refFeaturesPerDesc.at(track.descType); + const feature::PointFeatures& nextFeatures = nextFeaturesPerDesc.at(track.descType); + const IndexT refFeatureId = track.featPerView.at(pair.reference); + const IndexT nextFeatureId = track.featPerView.at(pair.next); + const Vec2 refpt = refFeatures[refFeatureId].coords().cast(); + const Vec2 nextpt = nextFeatures[nextFeatureId].coords().cast(); + + const Vec2 refptu = refIntrinsics->get_ud_pixel(refpt); + const Vec2 nextptu = nextIntrinsics->get_ud_pixel(nextpt); + + Vec3 X; + multiview::TriangulateDLT(P1, refptu, P2, nextptu, &X); + + if (X(2) < 0.0) + { + continue; + } + + sfmData::Landmark landmark; + landmark.descType = track.descType; + landmark.X = X; + + sfmData::Observation refObs; + refObs.id_feat = refFeatureId; + refObs.scale = refFeatures[refFeatureId].scale(); + refObs.x = refpt; + + sfmData::Observation nextObs; + nextObs.id_feat = nextFeatureId; + nextObs.scale = nextFeatures[nextFeatureId].scale(); + nextObs.x = nextpt; + + landmark.observations[pair.reference] = refObs; + landmark.observations[pair.next] = nextObs; + + sfmData.getLandmarks()[trackId] = landmark; + } + + return true; +} + +int aliceVision_main(int argc, char** argv) +{ + // command-line parameters + std::string sfmDataFilename; + std::string sfmDataOutputFilename; + std::vector featuresFolders; + std::string tracksFilename; + std::string pairsDirectory; + + // user optional parameters + std::string describerTypesName = feature::EImageDescriberType_enumToString(feature::EImageDescriberType::SIFT); + std::pair initialPairString("", ""); + + const double maxEpipolarDistance = 4.0; + const double minAngle = 5.0; + + int randomSeed = std::mt19937::default_seed; + + po::options_description requiredParams("Required parameters"); + requiredParams.add_options() + ("input,i", po::value(&sfmDataFilename)->required(), "SfMData file.") + ("output,o", po::value(&sfmDataOutputFilename)->required(), "SfMData output file.") + ("tracksFilename,t", po::value(&tracksFilename)->required(), "Tracks file.") + ("pairs,p", po::value(&pairsDirectory)->required(), "Path to the pairs directory.") + ("featuresFolders,f", po::value>(&featuresFolders)->multitoken(), "Path to folder(s) containing the extracted features.") + ("describerTypes,d", po::value(&describerTypesName)->default_value(describerTypesName),feature::EImageDescriberType_informations().c_str()); + + CmdLine cmdline("AliceVision pairsEstimations"); + + cmdline.add(requiredParams); + if(!cmdline.execute(argc, argv)) + { + return EXIT_FAILURE; + } + + // set maxThreads + HardwareContext hwc = cmdline.getHardwareContext(); + omp_set_num_threads(hwc.getMaxThreads()); + + // load input SfMData scene + sfmData::SfMData sfmData; + if(!sfmDataIO::Load(sfmData, sfmDataFilename, sfmDataIO::ESfMData::ALL)) + { + ALICEVISION_LOG_ERROR("The input SfMData file '" + sfmDataFilename + "' cannot be read."); + return EXIT_FAILURE; + } + + + if (sfmData.getValidViews().size() >= 2) + { + ALICEVISION_LOG_INFO("SfmData has already an initialization"); + return EXIT_SUCCESS; + } + + + // get imageDescriber type + const std::vector describerTypes = + feature::EImageDescriberType_stringToEnums(describerTypesName); + + + // features reading + feature::FeaturesPerView featuresPerView; + ALICEVISION_LOG_INFO("Load features"); + if(!sfm::loadFeaturesPerView(featuresPerView, sfmData, featuresFolders, describerTypes)) + { + ALICEVISION_LOG_ERROR("Invalid features."); + return EXIT_FAILURE; + } + + // Load tracks + ALICEVISION_LOG_INFO("Load tracks"); + std::ifstream tracksFile(tracksFilename); + if(tracksFile.is_open() == false) + { + ALICEVISION_LOG_ERROR("The input tracks file '" + tracksFilename + "' cannot be read."); + return EXIT_FAILURE; + } + std::stringstream buffer; + buffer << tracksFile.rdbuf(); + boost::json::value jv = boost::json::parse(buffer.str()); + track::TracksMap mapTracks(track::flat_map_value_to(jv)); + + // Compute tracks per view + ALICEVISION_LOG_INFO("Estimate tracks per view"); + track::TracksPerView mapTracksPerView; + for(const auto& viewIt : sfmData.views) + { + // create an entry in the map + mapTracksPerView[viewIt.first]; + } + track::computeTracksPerView(mapTracks, mapTracksPerView); + + + //Result of pair estimations are stored in multiple files + std::vector reconstructedPairs; + const std::regex regex("pairs\\_[0-9]+\\.json"); + for(fs::directory_entry & file : boost::make_iterator_range(fs::directory_iterator(pairsDirectory), {})) + { + if (!std::regex_search(file.path().string(), regex)) + { + continue; + } + + std::ifstream inputfile(file.path().string()); + + boost::json::error_code ec; + std::vector values = readJsons(inputfile, ec); + for (const boost::json::value & value : values) + { + std::vector localVector = boost::json::value_to>(value); + reconstructedPairs.insert(reconstructedPairs.end(), localVector.begin(), localVector.end()); + } + } + + + //Check all pairs + ALICEVISION_LOG_INFO("Give a score to all pairs"); + int count = 0; + + double bestScore = 0.0; + sfm::ReconstructedPair bestPair; + std::vector bestUsedTracks; + + for (const sfm::ReconstructedPair & pair: reconstructedPairs) + { + std::vector usedTracks; + double angle = 0.0; + if (!estimatePairAngle(sfmData, pair, mapTracks, mapTracksPerView, featuresPerView, maxEpipolarDistance, angle, usedTracks)) + { + continue; + } + + if (radianToDegree(angle) < minAngle) + { + continue; + } + + double refScore = computeScore(featuresPerView, mapTracks, usedTracks, pair.reference, 16); + double nextScore = computeScore(featuresPerView, mapTracks, usedTracks, pair.next, 16); + + double score = std::min(refScore, nextScore) * radianToDegree(angle); + + if (score > bestScore) + { + bestPair = pair; + bestScore = score; + bestUsedTracks = usedTracks; + } + } + + if (!buildSfmData(sfmData, bestPair, mapTracks, featuresPerView, bestUsedTracks)) + { + return EXIT_FAILURE; + } + + sfmDataIO::Save(sfmData, sfmDataOutputFilename, sfmDataIO::ESfMData::ALL); + + return EXIT_SUCCESS; +}