// 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/. #ifndef ALICEVISION_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_ #define ALICEVISION_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_ #include "aliceVision/numeric/numeric.hpp" #include "aliceVision/linearProgramming/ISolver.hpp" #include #include #include //-- //- Implementation of algorithm from Paper titled : //- [1] "Multiple-View Geometry under the L_\infty Norm." //- Author : Fredrik Kahl, Richard Hartley. //- Date : 9 sept 2008. //- [2] "Multiple View Geometry and the L_\infty -norm" //- Author : Fredrik Kahl //- ICCV 2005. //-- namespace aliceVision { namespace lInfinityCV { using namespace linearProgramming; //-- Estimate the translation and the structure // from image points coordinates and camera rotations. // - Estimation of Ci from Ri and xij // [1] -> 6.1 Cameras with Known Rotation // // - This implementation Use L1 norm instead of the L2 norm of // the paper, it allows to use standard standard LP // (simplex) instead of using SOCP (second order cone programming). // Implementation by Pierre Moulon // // // This implementation handle noisy measurement by adding a slack // variables for each x,y,z residual. // Based on idea expressed in (See Algorithm 2.0 of [3]): // [3] Outlier Removal Using Duality. // Carl Olsson, Anders Eriksson and Richard Hartley, Richard. // CVPR 2010. /// Encode translation and structure linear program with slack variables /// in order to handle noisy measurements. inline void EncodeTiXi_withNoise(const Mat& M, // Scene representation const std::vector& Ri, double sigma, // Start upper bound sRMat& A, Vec& C, std::vector& vec_sign, std::vector& vec_costs, std::vector>& vec_bounds) { // Build Constraint matrix. const size_t Ncam = (size_t)M.row(3).maxCoeff() + 1; const size_t N3D = (size_t)M.row(2).maxCoeff() + 1; const size_t Nobs = M.cols(); assert(Ncam == Ri.size()); A.resize(5 * Nobs + 3, 3 * (Ncam + N3D + Nobs)); C.resize(5 * Nobs + 3, 1); C.fill(0.0); vec_sign.resize(5 * Nobs + 3); const size_t transStart = 0; const size_t pointStart = transStart + 3 * Ncam; const size_t offsetStart = pointStart + 3 * N3D; #define TVAR(i, el) (0 + 3 * (i) + (el)) #define XVAR(j, el) (pointStart + 3 * (j) + (el)) #define OFSVAR(k, el) (offsetStart + 3 * (k) + (el)) // Set objective coefficients. vec_costs = std::vector(3 * (Ncam + N3D + Nobs), 0.0); for (size_t k = 0; k < Nobs; ++k) { vec_costs[OFSVAR(k, 0)] = 1.0; vec_costs[OFSVAR(k, 1)] = 1.0; vec_costs[OFSVAR(k, 2)] = 1.0; } // By default set free variable: vec_bounds = std::vector>(3 * (Ncam + N3D + Nobs)); fill(vec_bounds.begin(), vec_bounds.end(), std::make_pair((double)-1e+30, (double)1e+30)); // Change the offset to be positive for (size_t k = 0; k < 3 * Nobs; ++k) vec_bounds[offsetStart + k].first = 0; size_t rowPos = 0; // Add the cheirality conditions (R_i*X_j + T_i)_3 >= 1 for (size_t k = 0; k < Nobs; ++k) { const size_t indexPt3D = M(2, k); const size_t indexCam = M(3, k); const Mat3& R = Ri[indexCam]; A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(2, 0); A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(2, 1); A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(2, 2); A.coeffRef(rowPos, TVAR(indexCam, 2)) = 1.0; A.coeffRef(rowPos, OFSVAR(k, 2)) = 1.0; C(rowPos) = 1.0; vec_sign[rowPos] = LPConstraints::LP_GREATER_OR_EQUAL; ++rowPos; } // end for (k) // Add conic constraint: for (size_t k = 0; k < Nobs; ++k) { const Vec2 pt = M.block<2, 1>(0, k); const double u = pt(0); const double v = pt(1); const size_t indexPt3D = M(2, k); const size_t indexCam = M(3, k); const Mat3& R = Ri[indexCam]; // x-residual => // (R_i*X_j + T_i)_1 / (R_i*X_j + T_i)_3 - u >= -sigma // (R_i*X_j + T_i)_1 - u * (R_i*X_j + T_i)_3 + sigma (R_i*X_j + T_i)_3 >= 0.0 // R_i_3 * (sigma-u) + R_i_1 + t_i_1 + t_i_3 * (sigma-u) >= 0 A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(0, 0) + (sigma - u) * R(2, 0); A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(0, 1) + (sigma - u) * R(2, 1); A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(0, 2) + (sigma - u) * R(2, 2); A.coeffRef(rowPos, TVAR(indexCam, 0)) = 1.0; A.coeffRef(rowPos, TVAR(indexCam, 2)) = sigma - u; A.coeffRef(rowPos, OFSVAR(k, 0)) = 1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LPConstraints::LP_GREATER_OR_EQUAL; ++rowPos; A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(0, 0) - (sigma + u) * R(2, 0); A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(0, 1) - (sigma + u) * R(2, 1); A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(0, 2) - (sigma + u) * R(2, 2); A.coeffRef(rowPos, TVAR(indexCam, 0)) = 1.0; A.coeffRef(rowPos, TVAR(indexCam, 2)) = -(sigma + u); A.coeffRef(rowPos, OFSVAR(k, 0)) = -1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LPConstraints::LP_LESS_OR_EQUAL; ++rowPos; // y-residual => A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(1, 0) + (sigma - v) * R(2, 0); A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(1, 1) + (sigma - v) * R(2, 1); A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(1, 2) + (sigma - v) * R(2, 2); A.coeffRef(rowPos, OFSVAR(k, 1)) = 1.0; A.coeffRef(rowPos, TVAR(indexCam, 1)) = 1.0; A.coeffRef(rowPos, TVAR(indexCam, 2)) = sigma - v; C(rowPos) = 0.0; vec_sign[rowPos] = LPConstraints::LP_GREATER_OR_EQUAL; ++rowPos; A.coeffRef(rowPos, XVAR(indexPt3D, 0)) = R(1, 0) - (sigma + v) * R(2, 0); A.coeffRef(rowPos, XVAR(indexPt3D, 1)) = R(1, 1) - (sigma + v) * R(2, 1); A.coeffRef(rowPos, XVAR(indexPt3D, 2)) = R(1, 2) - (sigma + v) * R(2, 2); A.coeffRef(rowPos, TVAR(indexCam, 1)) = 1.0; A.coeffRef(rowPos, TVAR(indexCam, 2)) = -(sigma + v); A.coeffRef(rowPos, OFSVAR(k, 1)) = -1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LPConstraints::LP_LESS_OR_EQUAL; ++rowPos; } // Fix the translation ambiguity. (set first cam at (0,0,0) // LP_EQUAL A.coeffRef(rowPos, TVAR(0, 0)) = 1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LPConstraints::LP_EQUAL; ++rowPos; A.coeffRef(rowPos, TVAR(0, 1)) = 1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LPConstraints::LP_EQUAL; ++rowPos; A.coeffRef(rowPos, TVAR(0, 2)) = 1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LPConstraints::LP_EQUAL; ++rowPos; #undef TVAR #undef XVAR #undef OFSVAR } struct TiXi_withNoise_L1_ConstraintBuilder { TiXi_withNoise_L1_ConstraintBuilder(const std::vector& vec_Ri, const Mat& M) { _M = M; _vec_Ri = vec_Ri; } /// Setup constraints for the translation and structure problem, /// in the LPConstraints object. bool Build(double gamma, LPConstraintsSparse& constraint) { EncodeTiXi_withNoise(_M, _vec_Ri, gamma, constraint._constraintMat, constraint._Cst_objective, constraint._vec_sign, constraint._vec_cost, constraint._vec_bounds); constraint._bminimize = true; //-- Setup additional information about the Linear Program constraint // We look for nb translations and nb 3D points. const size_t N3D = (size_t)_M.row(2).maxCoeff() + 1; const size_t Nobs = _M.cols(); const size_t NNoiseVar = Nobs; // noise over Zdepth, (x,y) residual const size_t Ncam = (size_t)_M.row(3).maxCoeff() + 1; constraint._nbParams = (Ncam + N3D + NNoiseVar) * 3; return true; } std::vector _vec_Ri; // Rotation matrix Mat _M; // M contains (X,Y,index3dPoint, indexCam)^T }; } // namespace lInfinityCV } // namespace aliceVision #endif // ALICEVISION_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_