diff --git a/BIBLIOGRAPHY.md b/BIBLIOGRAPHY.md index 46e05095f8..a14584aecd 100644 --- a/BIBLIOGRAPHY.md +++ b/BIBLIOGRAPHY.md @@ -119,3 +119,6 @@ [Bok] **Automated checkerboard detection and indexing using circular boundaries** Yunsu Bok, Hyowon Ha, In So Kweon. + +[Zhang] **A flexible new technique for camera calibration** + Zhengyou Zhang (1998). diff --git a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp index 28da9ba8c2..526b702ab5 100644 --- a/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp +++ b/src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp @@ -289,8 +289,8 @@ ceres::CostFunction* createRigCostFunctionFromIntrinsics(const IntrinsicBase* in return new ceres::AutoDiffCostFunction( new ResidualErrorFunctor_Pinhole3DEClassicLD(w, h, obsUndistorted)); case EINTRINSIC::PINHOLE_CAMERA_3DEANAMORPHIC4: - return new ceres::AutoDiffCostFunction( - new ResidualErrorFunctor_Pinhole3DEAnamorphic4(w, h, obsUndistorted)); + return new ceres::AutoDiffCostFunction( + new ResidualErrorFunctor_Pinhole(w, h, obsUndistorted)); case EINTRINSIC::PINHOLE_CAMERA_BROWN: return new ceres::AutoDiffCostFunction( new ResidualErrorFunctor_PinholeBrownT2(w, h, obsUndistorted)); diff --git a/src/aliceVision/sfmDataIO/sfmDataIO.cpp b/src/aliceVision/sfmDataIO/sfmDataIO.cpp index d240fef206..f7da385bdb 100644 --- a/src/aliceVision/sfmDataIO/sfmDataIO.cpp +++ b/src/aliceVision/sfmDataIO/sfmDataIO.cpp @@ -29,12 +29,19 @@ namespace sfmDataIO { bool ValidIds(const sfmData::SfMData& sfmData, ESfMData partFlag) { const bool bCheck_Intrinsic = (partFlag & INTRINSICS); + const bool bCheck_Rig = (partFlag & EXTRINSICS); + // Collect intrinsic IDs declared std::set intrinsicIdsDeclared; transform(sfmData.getIntrinsics().begin(), sfmData.getIntrinsics().end(), std::inserter(intrinsicIdsDeclared, intrinsicIdsDeclared.begin()), stl::RetrieveKey()); - // Collect id_intrinsic referenced from views + // Collect rig IDs declared + std::set rigIdsDeclared; + transform(sfmData.getRigs().begin(), sfmData.getRigs().end(), + std::inserter(rigIdsDeclared, rigIdsDeclared.begin()), stl::RetrieveKey()); + + // Collect intrinsic IDs referenced from views std::set intrinsicIdsReferenced; for(const auto& v: sfmData.getViews()) { @@ -42,11 +49,20 @@ bool ValidIds(const sfmData::SfMData& sfmData, ESfMData partFlag) intrinsicIdsReferenced.insert(id_intrinsic); } - // We may have some views with undefined Intrinsics, + // Collect rig IDs referenced from views + std::set rigIdsReferenced; + for(const auto& v: sfmData.getViews()) + { + const IndexT id_rig = v.second.get()->getRigId(); + rigIdsReferenced.insert(id_rig); + } + + // We may have some views with undefined intrinsic/rig, // so erase the UndefinedIndex value if exist. intrinsicIdsReferenced.erase(UndefinedIndexT); + rigIdsReferenced.erase(UndefinedIndexT); - // Check if defined intrinsic & extrinsic are at least connected to views + // Check if defined intrinsics are at least connected to views bool bRet = true; if(bCheck_Intrinsic && intrinsicIdsDeclared != intrinsicIdsReferenced) { @@ -64,6 +80,23 @@ bool ValidIds(const sfmData::SfMData& sfmData, ESfMData partFlag) bRet = false; // error } + // Check if defined rigs are at least connected to views + if(bCheck_Rig && rigIdsDeclared != rigIdsReferenced) + { + ALICEVISION_LOG_WARNING("The number of rigs is incoherent:"); + ALICEVISION_LOG_WARNING(rigIdsDeclared.size() << " rigs declared and " << rigIdsReferenced.size() << " rigs used."); + std::set undefinedRigIds; + // undefinedRigIds = rigIdsReferenced - rigIdsDeclared + std::set_difference(rigIdsReferenced.begin(), rigIdsReferenced.end(), + rigIdsDeclared.begin(), rigIdsDeclared.end(), + std::inserter(undefinedRigIds, undefinedRigIds.begin())); + // If undefinedRigIds is not empty, + // some rigs are used in Views but never declared. + // So the file structure is invalid and may create troubles. + if(!undefinedRigIds.empty()) + bRet = false; // error + } + return bRet; } diff --git a/src/software/pipeline/CMakeLists.txt b/src/software/pipeline/CMakeLists.txt index 1dc853624e..3b07d86b6a 100644 --- a/src/software/pipeline/CMakeLists.txt +++ b/src/software/pipeline/CMakeLists.txt @@ -244,6 +244,22 @@ if(ALICEVISION_BUILD_SFM) Boost::json ) + alicevision_add_software(aliceVision_checkerboardCalibration + SOURCE main_checkerboardCalibration.cpp + FOLDER ${FOLDER_SOFTWARE_PIPELINE} + LINKS aliceVision_dataio + aliceVision_image + aliceVision_calibration + aliceVision_system + aliceVision_cmdline + aliceVision_sfmDataIO + aliceVision_multiview + aliceVision_sfm + Boost::program_options + Boost::json + Boost::boost + ) + # Calibrate a rig alicevision_add_software(aliceVision_rigCalibration SOURCE main_rigCalibration.cpp diff --git a/src/software/pipeline/main_checkerboardCalibration.cpp b/src/software/pipeline/main_checkerboardCalibration.cpp new file mode 100644 index 0000000000..413458c65b --- /dev/null +++ b/src/software/pipeline/main_checkerboardCalibration.cpp @@ -0,0 +1,542 @@ +// This file is part of the AliceVision project. +// Copyright (c) 2022 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/. + + +// This application tries to estimate the intrinsics and extrinsics of a set of images. +// It is assumed that for each image we have a result of the checkerboard detector. +// It is assumed that the distortion is at least approximately known or calibrated. +// It is assumed that we have several views with different poses orientation of the same checkerboard. +// It is assumed we know the square size of the checkerboard. + +#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 + +namespace po = boost::program_options; +using namespace aliceVision; + +Eigen::Matrix computeV(const Eigen::Matrix3d& H, int i, int j) +{ + Eigen::Matrix v; + + v(0, 0) = H(0, i) * H(0, j); + v(0, 1) = H(0, i) * H(1, j) + H(1, i) * H(0, j); + v(0, 2) = H(1, i) * H(1, j); + v(0, 3) = H(2, i) * H(0, j) + H(0, i) * H(2, j); + v(0, 4) = H(2, i) * H(1, j) + H(1, i) * H(2, j); + v(0, 5) = H(2, i) * H(2, j); + + return v; +} + +bool estimateIntrinsicsPoses(sfmData::SfMData& sfmData, + std::map& boardsAllImages, + const double squareSize) +{ + if (boardsAllImages.size() < 2) + { + ALICEVISION_LOG_ERROR("At least 2 views are needed"); + return EXIT_FAILURE; + } + + // Calibrate each intrinsic independently + Eigen::Matrix indices; + size_t global_checkerboard_w = 0; + size_t global_checkerboard_h = 0; + bool first = true; + for (auto& pi : sfmData.getIntrinsics()) + { + IndexT intrinsicId = pi.first; + + // Convert to pinhole + std::shared_ptr& intrinsicPtr = pi.second; + std::shared_ptr cameraPinhole = std::dynamic_pointer_cast(intrinsicPtr); + if (!cameraPinhole) + { + ALICEVISION_LOG_ERROR("Only work for pinhole cameras"); + return false; + } + + ALICEVISION_LOG_INFO("Processing Intrinsic " << intrinsicId); + + std::map, int> checkerboard_dims; + std::map homographies; + for (auto& pv : sfmData.getViews()) + { + if (pv.second->getIntrinsicId() != intrinsicId) continue; + + const calibration::CheckerDetector& detector = boardsAllImages[pv.first]; + if (detector.getBoards().size() != 1) + { + ALICEVISION_LOG_WARNING("The view " << pv.first << " has either 0 or more than 1 checkerboard found."); + continue; + } + + // Build a list of points (meter to undistorted pixels) + std::vector refpts; + std::vector points; + const auto& bs = detector.getBoards(); + const auto& b = bs[0]; + + const auto dim = std::make_pair(size_t(b.cols()), size_t(b.rows())); + if (checkerboard_dims.find(dim) == checkerboard_dims.end()) + { + checkerboard_dims[dim] = 1; + } + else + { + ++checkerboard_dims[dim]; + } + + for (int i = 0; i < b.rows(); i++) + { + for (int j = 0; j < b.cols(); j++) + { + IndexT cid = b(i, j); + if (b(i, j) == UndefinedIndexT) + { + continue; + } + + + Eigen::Vector2d refpt; + refpt(0) = double(j) * squareSize; + refpt(1) = double(i) * squareSize; + + Eigen::Vector2d curpt; + curpt = detector.getCorners()[cid].center; + curpt = cameraPinhole->get_ud_pixel(curpt); + + refpts.push_back(refpt); + points.push_back(curpt); + } + } + + // Estimate homography from this list of points + Eigen::MatrixXd Mref(2, refpts.size()); + for (int idx = 0; idx < refpts.size(); idx++) + { + Mref(0, idx) = refpts[idx].x(); + Mref(1, idx) = refpts[idx].y(); + } + + Eigen::MatrixXd Mcur(2, points.size()); + for (int idx = 0; idx < points.size(); idx++) + { + Mcur(0, idx) = points[idx].x(); + Mcur(1, idx) = points[idx].y(); + } + + using KernelType = multiview::RelativePoseKernel; + + KernelType kernel(Mref, 1.0, 1.0, Mcur, cameraPinhole->w(), cameraPinhole->h(), false); // configure as point to point error model. + + robustEstimation::Mat3Model model; + std::mt19937 generator; + std::vector vec_inliers; + + robustEstimation::ACRANSAC(kernel, generator, vec_inliers, 1024, &model, std::numeric_limits::infinity()); + Eigen::Matrix3d H = model.getMatrix(); + + if (vec_inliers.size() < 10) continue; + + homographies[pv.first] = H; + } + + // Now we use algorithm from [Zhang] + Eigen::MatrixXd V(homographies.size() * 2, 6); + + size_t pos = 0; + for (const auto& pH : homographies) + { + const Eigen::Matrix3d& H = pH.second; + + V.block<1, 6>(pos * 2, 0) = computeV(H, 0, 1); + V.block<1, 6>(pos * 2 + 1, 0) = computeV(H, 0, 0) - computeV(H, 1, 1); + pos++; + } + + Eigen::JacobiSVD svd(V, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::MatrixXd N = svd.matrixV(); + Eigen::VectorXd n = N.col(N.cols() - 1); + + Eigen::Matrix3d B; + B(0, 0) = n(0); + B(0, 1) = n(1); + B(1, 0) = n(1); + B(1, 1) = n(2); + B(0, 2) = n(3); + B(2, 0) = n(3); + B(1, 2) = n(4); + B(2, 1) = n(4); + B(2, 2) = n(5); + + // Extract intrinsics from B + double v0 = (B(0, 1) * B(0, 2) - B(0, 0) * B(1, 2)) / (B(0, 0) * B(1, 1) - B(0, 1) * B(0, 1)); + double lambda = B(2, 2) - (B(0, 2) * B(0, 2) + v0 * (B(0, 1) * B(0, 2) - B(0, 0) * B(1, 2))) / B(0, 0); + double alpha = sqrt(lambda / B(0, 0)); + double beta = sqrt(lambda * B(0, 0) / (B(0, 0) * B(1, 1) - B(0, 1) * B(0, 1))); + double gamma = -B(0, 1) * alpha * alpha * beta / lambda; + double u0 = (gamma * v0 / beta) - (B(0, 2) * alpha * alpha / lambda); + + Eigen::Matrix3d A = Eigen::Matrix3d::Identity(); + A(0, 0) = alpha; + A(1, 1) = beta; + A(0, 1) = gamma; + A(0, 2) = u0; + A(1, 2) = v0; + + // Initialize camera intrinsics + cameraPinhole->setK(A); + + // Estimate checkerboard dimensions + size_t checkerboard_w = 0; + size_t checkerboard_h = 0; + int max_count = 0; + for (const auto& [dim, count] : checkerboard_dims) + { + if (count > max_count) + { + max_count = count; + checkerboard_w = dim.first; + checkerboard_h = dim.second; + } + } + + if (first) + { + global_checkerboard_w = checkerboard_w; + global_checkerboard_h = checkerboard_h; + + // Create landmarks + indices = Eigen::Matrix(global_checkerboard_h, global_checkerboard_w); + IndexT posLandmark = 0; + for (int i = 0; i < global_checkerboard_h; i++) + { + for (int j = 0; j < global_checkerboard_w; j++) + { + indices(i, j) = posLandmark; + + sfmData::Landmark l(Vec3(squareSize * double(j), squareSize * double(i), 0.0), feature::EImageDescriberType::SIFT); + sfmData.getLandmarks()[posLandmark] = l; + posLandmark++; + } + } + + first = false; + } + else + { + if (checkerboard_w != global_checkerboard_w || checkerboard_h != global_checkerboard_h) + { + ALICEVISION_LOG_WARNING("Inconsistent checkerboard size."); + } + } + + // Initialize poses for each view using linear method + Eigen::Matrix3d Ainv = A.inverse(); + for (const auto& pH : homographies) + { + const Eigen::Matrix3d& H = pH.second; + Eigen::Matrix T; + Eigen::Matrix3d M; + + double tlambda = 1.0 / (Ainv * H.col(1)).norm(); + + T.col(3) = tlambda * Ainv * H.col(2); + if (T(2, 3) < 0.0) + { + tlambda *= -1.0; + } + T.col(3) = tlambda * Ainv * H.col(2); + + M.col(0) = tlambda * Ainv * H.col(0); + M.col(1) = tlambda * Ainv * H.col(1); + M.col(2) = M.col(0).cross(M.col(1)); + + Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); + T.block<3, 3>(0, 0) = svd.matrixU() * svd.matrixV().transpose(); + + geometry::Pose3 pose(T.block<3, 3>(0, 0), -T.block<3, 3>(0, 0).transpose() * T.col(3)); + sfmData::CameraPose cp; + cp.setTransform(pose); + + auto view = sfmData.getViews().at(pH.first); + sfmData.getPoses()[view->getPoseId()] = cp; + } + + // Get residuals + for (auto& pH : homographies) + { + IndexT viewId = pH.first; + + const calibration::CheckerDetector& detector = boardsAllImages[viewId]; + const auto& bs = detector.getBoards(); + const auto& b = bs[0]; + + if (b.cols() != global_checkerboard_w || b.rows() != global_checkerboard_h) + { + continue; + } + + const auto& corners = detector.getCorners(); + + auto& curview = sfmData.getViews().at(viewId); + + for (int i = 0; i < global_checkerboard_h; i++) + { + for (int j = 0; j < global_checkerboard_w; j++) + { + IndexT idxlandmark = indices(i, j); + if (idxlandmark == UndefinedIndexT) + continue; + + IndexT idxcorner = b(i, j); + if (idxcorner == UndefinedIndexT) + continue; + + Vec2 p = corners[idxcorner].center; + + // Add observation + sfmData::Observation obs(p, idxlandmark, 1.0); + sfmData.getLandmarks()[idxlandmark].observations[viewId] = obs; + } + } + } + + // Compute non linear refinement + sfm::BundleAdjustmentCeres::CeresOptions options; + options.summary = true; + sfm::BundleAdjustmentCeres ba(options); + sfm::BundleAdjustment::ERefineOptions boptions = + sfm::BundleAdjustment::ERefineOptions::REFINE_ROTATION | + sfm::BundleAdjustment::ERefineOptions::REFINE_TRANSLATION | + sfm::BundleAdjustment::ERefineOptions::REFINE_INTRINSICS_ALL; + if (!ba.adjust(sfmData, boptions)) + { + ALICEVISION_LOG_ERROR("Failed to calibrate"); + return false; + } + } + + return true; +} + + +bool estimateRigs(sfmData::SfMData& sfmData) +{ + // Calibrate rigs + if (sfmData.getRigs().size() > 0) + { + // Initialize rig poses + sfmData::Poses rigPoses; + for (auto& pv : sfmData.getViews()) + { + auto view = pv.second; + if (view->isPartOfRig()) + { + const IndexT rigId = view->getRigId(); + const IndexT subPoseId = view->getSubPoseId(); + sfmData::Rig& rig = sfmData.getRigs().at(rigId); + sfmData::RigSubPose& subPose = rig.getSubPose(subPoseId); + if (subPoseId == 0) + { + if (sfmData.getPoses().find(view->getPoseId()) == sfmData.getPoses().end()) + { + continue; + } + sfmData::CameraPose absPose = sfmData.getPoses().at(view->getPoseId()); + rigPoses[rigId] = absPose; + subPose.pose = geometry::Pose3(); // identity + subPose.status = sfmData::ERigSubPoseStatus::CONSTANT; + } + } + } + + // Initialize sub-poses + for (auto& pv : sfmData.getViews()) + { + auto view = pv.second; + if (view->isPartOfRig()) + { + const IndexT rigId = view->getRigId(); + const IndexT subPoseId = view->getSubPoseId(); + sfmData::Rig& rig = sfmData.getRigs().at(rigId); + sfmData::RigSubPose& subPose = rig.getSubPose(subPoseId); + if (subPoseId > 0) + { + if (sfmData.getPoses().find(view->getPoseId()) == sfmData.getPoses().end()) + { + continue; + } + sfmData::CameraPose absPose = sfmData.getPoses().at(view->getPoseId()); + sfmData::CameraPose rigPose = rigPoses[rigId]; + sfmData.getPoses()[view->getPoseId()] = rigPose; + subPose.pose = absPose.getTransform() * rigPose.getTransform().inverse(); + subPose.status = sfmData::ERigSubPoseStatus::ESTIMATED; + } + } + } + + // Turn off independent pose flag on views + for (auto& pv : sfmData.getViews()) + { + auto view = pv.second; + if (view->isPartOfRig()) + { + view->setIndependantPose(false); + } + } + + // Compute non-linear refinements + sfm::BundleAdjustmentCeres::CeresOptions options; + options.summary = true; + sfm::BundleAdjustmentCeres ba(options); + sfm::BundleAdjustment::ERefineOptions boptions = + sfm::BundleAdjustment::ERefineOptions::REFINE_ROTATION | + sfm::BundleAdjustment::ERefineOptions::REFINE_TRANSLATION | + sfm::BundleAdjustment::ERefineOptions::REFINE_INTRINSICS_ALL; + if (!ba.adjust(sfmData, boptions)) + { + ALICEVISION_LOG_ERROR("Failed to calibrate"); + return false; + } + } + + return true; +} + + +int aliceVision_main(int argc, char* argv[]) +{ + std::string sfmInputDataFilepath; + std::string checkerBoardsPath; + std::string sfmOutputDataFilepath; + + double squareSize = 10.; + + po::options_description requiredParams("Required parameters"); + requiredParams.add_options() + ("input,i", po::value(&sfmInputDataFilepath)->required(), "SfMData file input.") + ("checkerboards,c", po::value(&checkerBoardsPath)->required(), "Checkerboards json files directory.") + ("output,o", po::value(&sfmOutputDataFilepath)->required(), "SfMData file output."); + + po::options_description optionalParams("Optional parameters"); + optionalParams.add_options() + ("squareSize,s", po::value(&squareSize)->default_value(squareSize), "Checkerboard square width in mm"); + + CmdLine cmdline("This program calibrates camera intrinsics and extrinsics.\n" + "AliceVision checkerboardCalibration"); + cmdline.add(requiredParams); + cmdline.add(optionalParams); + if (!cmdline.execute(argc, argv)) + { + return EXIT_FAILURE; + } + + // Load sfmData from disk + sfmData::SfMData sfmData; + if (!sfmDataIO::Load(sfmData, sfmInputDataFilepath, sfmDataIO::ESfMData(sfmDataIO::ALL))) + { + ALICEVISION_LOG_ERROR("The input SfMData file '" << sfmInputDataFilepath << "' cannot be read."); + return EXIT_FAILURE; + } + + // Load the checkerboards + std::map < IndexT, calibration::CheckerDetector> boardsAllImages; + for (auto& pv : sfmData.getViews()) + { + IndexT viewId = pv.first; + + // Read the json file + std::stringstream ss; + ss << checkerBoardsPath << "/" << "checkers_" << viewId << ".json"; + std::ifstream inputfile(ss.str()); + if (inputfile.is_open() == false) continue; + + std::stringstream buffer; + buffer << inputfile.rdbuf(); + boost::json::value jv = boost::json::parse(buffer.str()); + + // Store the checkerboard + calibration::CheckerDetector detector(boost::json::value_to(jv)); + boardsAllImages[viewId] = detector; + } + + // Calibrate intrinsics and poses + if (!estimateIntrinsicsPoses(sfmData, boardsAllImages, squareSize)) + { + return EXIT_FAILURE; + } + + // Calibrate rigs + if (!estimateRigs(sfmData)) + { + return EXIT_FAILURE; + } + + // Mark all intrinsics as calibrated + for (auto& pi : sfmData.getIntrinsics()) + { + std::shared_ptr& intrinsicPtr = pi.second; + intrinsicPtr->setInitializationMode(camera::EInitMode::CALIBRATED); + } + + // Mark all rig sub-poses as constant + for (auto& pr : sfmData.getRigs()) + { + sfmData::Rig& rig = pr.second; + for (auto& subPose : rig.getSubPoses()) + { + subPose.status = sfmData::ERigSubPoseStatus::CONSTANT; + } + } + + // Save sfmData to disk + if (!sfmDataIO::Save(sfmData, sfmOutputDataFilepath, sfmDataIO::ESfMData(sfmDataIO::ALL))) + { + ALICEVISION_LOG_ERROR("The output SfMData file '" << sfmOutputDataFilepath << "' cannot be read."); + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} diff --git a/src/software/utils/main_applyCalibration.cpp b/src/software/utils/main_applyCalibration.cpp index ecad5d13f7..8c3456255d 100644 --- a/src/software/utils/main_applyCalibration.cpp +++ b/src/software/utils/main_applyCalibration.cpp @@ -20,13 +20,12 @@ // 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_MAJOR 2 #define ALICEVISION_SOFTWARE_VERSION_MINOR 0 namespace po = boost::program_options; using namespace aliceVision; - int aliceVision_main(int argc, char **argv) { // command-line parameters @@ -64,7 +63,7 @@ int aliceVision_main(int argc, char **argv) if (sfmDataCalibratedFilename.empty()) { // Save sfmData to disk - if (!sfmDataIO::Save(sfmData, outSfMDataFilename, sfmDataIO::ESfMData(sfmDataIO::ALL))) + if (!sfmDataIO::Save(sfmData, outSfMDataFilename, sfmDataIO::ESfMData::ALL)) { ALICEVISION_LOG_ERROR("The output SfMData file '" << outSfMDataFilename << "' cannot be written."); return EXIT_FAILURE; @@ -75,38 +74,118 @@ int aliceVision_main(int argc, char **argv) // Load calibrated scene sfmData::SfMData sfmDataCalibrated; - if (!sfmDataIO::Load(sfmDataCalibrated, sfmDataCalibratedFilename, sfmDataIO::ESfMData::INTRINSICS)) + if (!sfmDataIO::Load(sfmDataCalibrated, sfmDataCalibratedFilename, sfmDataIO::ESfMData::ALL)) { ALICEVISION_LOG_ERROR("The calibrated SfMData file '" << sfmDataCalibratedFilename << "' cannot be read"); return EXIT_FAILURE; } - // Check that there is exactly one calibrated intrinsic + // Detect calibration setup const auto& calibratedIntrinsics = sfmDataCalibrated.getIntrinsics(); - if (calibratedIntrinsics.size() == 0) + + const bool isMonoCam = calibratedIntrinsics.size() == 1; + const bool isMultiCam = calibratedIntrinsics.size() > 1 && sfmDataCalibrated.getRigs().size() == 1; + + if (!isMonoCam && !isMultiCam) { - ALICEVISION_LOG_ERROR("The calibrated SfMData does not have any intrinsics"); + ALICEVISION_LOG_ERROR("Unknown calibration setup"); return EXIT_FAILURE; } - if (calibratedIntrinsics.size() > 1) + + // Apply rig calibration + if (isMultiCam) { - ALICEVISION_LOG_ERROR("The calibrated SfMData has more than 1 intrinsics"); - return EXIT_FAILURE; - } + if (sfmData.getRigs().size() != 1) + { + ALICEVISION_LOG_ERROR("There must be exactly 1 rig to apply rig calibration"); + return EXIT_FAILURE; + } - std::shared_ptr calibratedIntrinsic - = std::dynamic_pointer_cast(calibratedIntrinsics.begin()->second); + // Retrieve calibrated sub-poses + const auto& calibratedRig = sfmDataCalibrated.getRigs().begin()->second; + const auto& calibratedSubPoses = calibratedRig.getSubPoses(); - if (calibratedIntrinsic->getDistortionInitializationMode() != camera::EInitMode::CALIBRATED) - { - ALICEVISION_LOG_ERROR("Intrinsic from calibrated SfMData is not calibrated"); - return EXIT_FAILURE; + // Retrieve input sub-poses + auto& rig = sfmData.getRigs().begin()->second; + auto& subPoses = rig.getSubPoses(); + + if (subPoses.size() != calibratedSubPoses.size()) + { + ALICEVISION_LOG_ERROR("Incoherent number of sub-poses"); + return EXIT_FAILURE; + } + + // Copy calibrated sub-poses + for (std::size_t idx = 0; idx < subPoses.size(); ++idx) + { + if (calibratedSubPoses[idx].status != sfmData::ERigSubPoseStatus::CONSTANT) continue; + + subPoses[idx] = calibratedSubPoses[idx]; + subPoses[idx].status = sfmData::ERigSubPoseStatus::ESTIMATED; + } + + // Turn off independent pose flag on views + for (auto& [viewId, view] : sfmData.getViews()) + { + if (view->isPartOfRig()) + { + view->setIndependantPose(false); + } + } } - // Overwrite input SfMData intrinsics with calibrated one + // Apply intrinsic and distortion calibration auto& intrinsics = sfmData.getIntrinsics(); for (const auto& [intrinsicId, intrinsic] : intrinsics) { + ALICEVISION_LOG_INFO("Processing intrinsic " << intrinsicId); + + // Find corresponding calibrated intrinsic depending on calibration setup + std::shared_ptr calibratedIntrinsic = nullptr; + if (isMonoCam) + { + calibratedIntrinsic = std::dynamic_pointer_cast(calibratedIntrinsics.begin()->second); + } + else if (isMultiCam) + { + IndexT subPoseId = UndefinedIndexT; + for (const auto& [viewId, view] : sfmData.getViews()) + { + if (view->getIntrinsicId() != intrinsicId) continue; + + subPoseId = view->getSubPoseId(); + break; + } + + bool found = false; + for (const auto& [calibIntrId, calibIntr] : calibratedIntrinsics) + { + if (found) break; + + for (const auto& [viewId, view] : sfmDataCalibrated.getViews()) + { + if (view->getIntrinsicId() != calibIntrId) continue; + + if (view->getSubPoseId() != subPoseId) continue; + + calibratedIntrinsic = std::dynamic_pointer_cast(calibIntr); + found = true; + break; + } + } + } + + if (!calibratedIntrinsic) + { + ALICEVISION_LOG_ERROR("Unable to find a corresponding calibrated intrinsic"); + return EXIT_FAILURE; + } + + const bool isIntrinsicCalibrated = calibratedIntrinsic->getInitializationMode() == camera::EInitMode::CALIBRATED; + const bool isDistortionCalibrated = calibratedIntrinsic->getDistortionInitializationMode() == camera::EInitMode::CALIBRATED; + + if (!isIntrinsicCalibrated && !isDistortionCalibrated) continue; + // Aspect ratio of input intrinsic const unsigned int width = intrinsic->w(); const unsigned int height = intrinsic->h(); @@ -124,18 +203,37 @@ int aliceVision_main(int argc, char **argv) return EXIT_FAILURE; } - // Create copy of calibrated intrinsic - std::shared_ptr newIntrinsic - = std::dynamic_pointer_cast(camera::createIntrinsic(calibratedIntrinsic->getType())); - newIntrinsic->assign(*calibratedIntrinsic); + // Copy original intrinsic + auto newIntrinsic = + std::dynamic_pointer_cast( + camera::createIntrinsic(intrinsic->getType())); + newIntrinsic->assign(*intrinsic); + + if (isIntrinsicCalibrated) + { + // Use calibrated focal length and offset + const double rx = static_cast(width) / static_cast(calibrationWidth); + const double ry = static_cast(height) / static_cast(calibrationHeight); + const double fx = calibratedIntrinsic->getScale().x() * rx; + const double fy = calibratedIntrinsic->getScale().y() * ry; + const double ox = calibratedIntrinsic->getOffset().x() * rx; + const double oy = calibratedIntrinsic->getOffset().y() * ry; + newIntrinsic->setScale({fx, fy}); + newIntrinsic->setOffset({ox, oy}); + newIntrinsic->setInitializationMode(camera::EInitMode::ESTIMATED); + } - // Apply dimensions of input intrinsic - newIntrinsic->setWidth(width); - newIntrinsic->setHeight(height); - auto undistortion = newIntrinsic->getUndistortion(); - if (undistortion) + if (isDistortionCalibrated) { + // Use calibrated distortion + newIntrinsic->setDistortionObject(nullptr); + auto calibratedUndistortion = calibratedIntrinsic->getUndistortion(); + auto undistortion = camera::createUndistortion( + calibratedUndistortion->getType()); undistortion->setSize(width, height); + undistortion->setParameters(calibratedUndistortion->getParameters()); + newIntrinsic->setUndistortionObject(undistortion); + newIntrinsic->setDistortionInitializationMode(camera::EInitMode::CALIBRATED); } // Overwrite intrinsic with new one