From 0a364011392667c74f01f0f62b1614764d41072f Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 29 Dec 2013 18:38:31 +0100 Subject: [PATCH 01/68] Add osi_clp as an external dependencie. #45 --- .gitmodules | 3 +++ src/dependencies/osi_clp | 1 + 2 files changed, 4 insertions(+) create mode 160000 src/dependencies/osi_clp diff --git a/.gitmodules b/.gitmodules index b2ac2f6139..cc83c78dd0 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "src/dependencies/OpenExif"] path = src/dependencies/OpenExif url = https://github.com/openMVG-thirdparty/OpenExif.git +[submodule "src/dependencies/osi_clp"] + path = src/dependencies/osi_clp + url = https://github.com/openMVG-thirdparty/osi_clp.git diff --git a/src/dependencies/osi_clp b/src/dependencies/osi_clp new file mode 160000 index 0000000000..3b9edb6fae --- /dev/null +++ b/src/dependencies/osi_clp @@ -0,0 +1 @@ +Subproject commit 3b9edb6fae07adb6a651cb2531d778fda5d335de From b295e1af3ca7aaa5b6ff059c3585b4c0f0018b7a Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 29 Dec 2013 18:44:13 +0100 Subject: [PATCH 02/68] Make osi_clp compile less binary. --- src/dependencies/osi_clp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/dependencies/osi_clp b/src/dependencies/osi_clp index 3b9edb6fae..c7af0c4b24 160000 --- a/src/dependencies/osi_clp +++ b/src/dependencies/osi_clp @@ -1 +1 @@ -Subproject commit 3b9edb6fae07adb6a651cb2531d778fda5d335de +Subproject commit c7af0c4b24156cfe0b5d804bf776df5c1e075dce From 76c7794794c924678c6a342a8c30e48053e13e97 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 29 Dec 2013 19:16:28 +0100 Subject: [PATCH 03/68] Initial revision of the linear_programming openMVG module. #46 --- BUILD | 9 + src/CMakeLists.txt | 30 ++ src/cmakeFindModules/FindMosek.cmake | 33 ++ src/openMVG/CMakeLists.txt | 2 + src/openMVG/linearProgramming/CMakeLists.txt | 34 ++ src/openMVG/linearProgramming/bisectionLP.hpp | 81 ++++ .../linearProgrammingInterface.hpp | 101 ++++ .../linearProgrammingMOSEK.cpp | 443 ++++++++++++++++++ .../linearProgrammingMOSEK.hpp | 53 +++ .../linearProgrammingOSI_X.hpp | 345 ++++++++++++++ .../linearProgramming_test.cpp | 220 +++++++++ src/openMVG/numeric/numeric.h | 6 + 12 files changed, 1357 insertions(+) create mode 100644 src/cmakeFindModules/FindMosek.cmake create mode 100644 src/openMVG/linearProgramming/CMakeLists.txt create mode 100644 src/openMVG/linearProgramming/bisectionLP.hpp create mode 100644 src/openMVG/linearProgramming/linearProgrammingInterface.hpp create mode 100644 src/openMVG/linearProgramming/linearProgrammingMOSEK.cpp create mode 100644 src/openMVG/linearProgramming/linearProgrammingMOSEK.hpp create mode 100644 src/openMVG/linearProgramming/linearProgrammingOSI_X.hpp create mode 100644 src/openMVG/linearProgramming/linearProgramming_test.cpp diff --git a/BUILD b/BUILD index 24ed43e4d5..9cf2d47e22 100644 --- a/BUILD +++ b/BUILD @@ -36,6 +36,15 @@ Setup the required external library. $ cd openMVG_Build $ cmake -DCMAKE_BUILD_TYPE=RELEASE . ../openMVG/src/ +=> In order to use the MOSEK backend for the linear programming oepnMVG module + - Check that you have an uptodate MOSEK licence, else openMVG MOSEK unit test will fail. + + $ cmake -DCMAKE_BUILD_TYPE=RELEASE + -DMOSEK_SEARCH_HEADER="~/Documents/Lib/mosek/6/tools/platform/linux64x86/h" + -DMOSEK_SEARCH_LIB="~/Documents/Lib/mosek/6/tools/platform/linux64x86/bin" + . ../openMVG/src/ + + If you want have an IDE openable project with codeblocks: $ cmake -G "CodeBlocks - Unix Makefiles" -DCMAKE_BUILD_TYPE=RELEASE . ../openMVG/src/ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 69d981ae19..840edcfd2d 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -75,6 +75,36 @@ IF (UNIX) ENDIF (EXIV2_FOUND) ENDIF (UNIX) +#=============================== +#---- LINEAR PROGRAMMING SOLVER +#=============================== + +#- Mosek (linear programming interface) +#=============================================================================== +FIND_PACKAGE(Mosek) +IF(MOSEK_FOUND) + ADD_DEFINITIONS(-DOPENMVG_HAVE_MOSEK) + INCLUDE_DIRECTORIES( + ${MOSEK_INCLUDE} + ./dependencies/osi_clp/Osi/src/OsiMsk/ + ) +ENDIF(MOSEK_FOUND) + +#- osi_clp (linear programming interface) +# ============================================================================== +ADD_SUBDIRECTORY(dependencies/osi_clp) +INCLUDE_DIRECTORIES( + ./dependencies/osi_clp/CoinUtils/src/ + ./dependencies/osi_clp/Clp/src/ + ./dependencies/osi_clp/Osi/src/Osi/ + ./dependencies/osi_clp/Clp/src/OsiClp/ +) + +#=============================== +#--END-- LINEAR PROGRAMMING SOLVER +#=============================== + + # ============================================================================== # Opencv is not used by openMVG but some samples show how to use openCV # and openMVG simultaneously diff --git a/src/cmakeFindModules/FindMosek.cmake b/src/cmakeFindModules/FindMosek.cmake new file mode 100644 index 0000000000..e15578497b --- /dev/null +++ b/src/cmakeFindModules/FindMosek.cmake @@ -0,0 +1,33 @@ + +# MOSEK library detection +FIND_PATH(MOSEK_INCLUDE NAMES mosek.h PATHS ${MOSEK_SEARCH_HEADER}) +FIND_LIBRARY(MOSEK_LIB NAMES libmosek libmosek.so libmosek64 libmosek64.so PATHS ${MOSEK_SEARCH_LIB}) + +IF (EXISTS ${MOSEK_INCLUDE} AND EXISTS ${MOSEK_LIB}) + + SET(MOSEK_FOUND true CACHE BOOL "USE MOSEK library") + MESSAGE("-- Found Mosek header in: ${MOSEK_INCLUDE}") + MESSAGE("-- Found Mosek library: ${MOSEK_LIB}") +# INCLUDE_DIRECTORIES( ${MOSEK_INCLUDE} ) + + IF (${UNIX}) + FIND_PACKAGE(Threads REQUIRED) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CMAKE_THREAD_LIBS_INIT}") + LIST(APPEND ${MOSEK_LIB} pthread) + ENDIF (${UNIX}) + +# ADD_DEFINITIONS(-DOPENMVG_HAVE_MOSEK) + +ELSE (EXISTS ${MOSEK_INCLUDE} AND EXISTS ${MOSEK_LIB}) + MESSAGE("-- Did not find MOSEK header") + IF (NOT EXISTS ${MOSEK_LIB}) + MESSAGE("-- Did not find MOSEK library") + ENDIF () +ENDIF (EXISTS ${MOSEK_INCLUDE} AND EXISTS ${MOSEK_LIB}) + +IF(NOT MOSEK_FOUND) + MESSAGE(STATUS "Could not find mosek library on this machine.") +ENDIF(NOT MOSEK_FOUND) + +MARK_AS_ADVANCED(MOSEK_FOUND) + diff --git a/src/openMVG/CMakeLists.txt b/src/openMVG/CMakeLists.txt index 8efb37b67c..ebda00e439 100644 --- a/src/openMVG/CMakeLists.txt +++ b/src/openMVG/CMakeLists.txt @@ -10,3 +10,5 @@ ADD_SUBDIRECTORY(bundle_adjustment) ADD_SUBDIRECTORY(exif_IO) ADD_SUBDIRECTORY(split) + +ADD_SUBDIRECTORY(linearProgramming) diff --git a/src/openMVG/linearProgramming/CMakeLists.txt b/src/openMVG/linearProgramming/CMakeLists.txt new file mode 100644 index 0000000000..488595a97e --- /dev/null +++ b/src/openMVG/linearProgramming/CMakeLists.txt @@ -0,0 +1,34 @@ +FILE( + GLOB + linearProgramming_headers + *.hpp +) +FILE( + GLOB + linearProgramming_cpp + *.cpp +) +FILE(GLOB_RECURSE REMOVEFILESUNITTEST *_test.cpp) + +SET_SOURCE_FILES_PROPERTIES(${linearProgramming_headers} PROPERTIES LANGUAGE CXX) + +#Remove the future main files +LIST(REMOVE_ITEM linearProgramming_cpp ${REMOVEFILESUNITTEST}) + +ADD_LIBRARY(openMVG_linearProgramming ${linearProgramming_headers} ${linearProgramming_cpp}) +TARGET_LINK_LIBRARIES(openMVG_linearProgramming + lib_OsiClpSolver # solver wrapper + lib_CoinUtils # container tools + lib_Osi # generic LP + lib_clp # Solver +) +IF (MOSEK_FOUND) + TARGET_LINK_LIBRARIES(openMVG_linearProgramming + pthread + ${MOSEK_LIB} + lib_Osi_Msk # OSI solver wrapper for the Mosek backend + ) +ENDIF (MOSEK_FOUND) + +UNIT_TEST(openMVG_linearProgramming linearProgramming "openMVG_linearProgramming") + diff --git a/src/openMVG/linearProgramming/bisectionLP.hpp b/src/openMVG/linearProgramming/bisectionLP.hpp new file mode 100644 index 0000000000..a661473987 --- /dev/null +++ b/src/openMVG/linearProgramming/bisectionLP.hpp @@ -0,0 +1,81 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_LINEAR_PROGRAMMING_BISECTIONLP_H_ +#define OPENMVG_LINEAR_PROGRAMMING_BISECTIONLP_H_ + +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include +#include +#include + +namespace openMVG { +namespace linearProgramming { + +/// Generic Bisection algorithm via Linear Programming. +/// Use dichotomy or mid-point best parameter that fit the solution. +/// http://en.wikipedia.org/wiki/Bisection_method +/// The bisection algorithm continue as long as +/// precision or max iteration number is not reach. +/// +template +bool BisectionLP( + LP_Solver & solver, + ConstraintBuilder & cstraintBuilder, + std::vector * parameters, + double gammaUp = 1.0, // Upper bound + double gammaLow = 0.0, // lower bound + double eps = 1e-8, // precision that stop dichotomy + const int maxIteration = 20, // max number of iteration + double * bestFeasibleGamma = NULL, // value of best bisection found value + bool bVerbose = false) +{ + int k = 0; + bool bModelFound = false; + ConstraintType constraint; + do + { + ++k; // One more iteration + + double gamma = (gammaLow + gammaUp) / 2.0; + + //-- Setup constraint and solver + cstraintBuilder.Build(gamma, constraint); + solver.setup( constraint ); + //-- + // Solving + bool bFeasible = solver.solve(); + //-- + + if (bFeasible) + { + gammaUp = gamma; + if (bestFeasibleGamma) + *bestFeasibleGamma = gamma; + solver.getSolution(*parameters); + bModelFound = true; + + if(bVerbose) + std::cout << "\n" << k<<"/"< eps); + + return bModelFound; +} + +} // namespace linearProgramming +} // namespace openMVG + + +#endif // OPENMVG_LINEAR_PROGRAMMING_BISECTIONLP_H_ diff --git a/src/openMVG/linearProgramming/linearProgrammingInterface.hpp b/src/openMVG/linearProgramming/linearProgrammingInterface.hpp new file mode 100644 index 0000000000..b02a808c5c --- /dev/null +++ b/src/openMVG/linearProgramming/linearProgrammingInterface.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_LINEAR_PROGRAMMING_INTERFACE_H_ +#define OPENMVG_LINEAR_PROGRAMMING_INTERFACE_H_ + +#include +#include +#include "openMVG/numeric/numeric.h" + +namespace openMVG { +namespace linearProgramming { + +/// Generic container for LP (Linear Programming problems). +/// Embed : +/// - objective function +/// - Constraints (coefficients, Sign, objective value), +/// - Bounds over parameter (<=, =, >=). +/// - minimize or maximize +/// +struct LP_Constraints +{ + enum eLP_SIGN + { + LP_LESS_OR_EQUAL = 1, // (<=) + LP_GREATER_OR_EQUAL = 2, // (>=) + LP_EQUAL = 3, // (=) + LP_FREE = 4 //only supported in MOSEK + }; + + LP_Constraints() { + _bminimize = false; + } + + int _nbParams; // The number of parameter/variable in constraint. + Mat _constraintMat; // Constraint under Matrix form. + Vec _Cst_objective; // Constraint objective value. + std::vector _vec_sign; // Constraint sign. + std::vector< std::pair > _vec_bounds; // parameter/variable bounds. + + bool _bminimize; // minimize is true or maximize is false. + std::vector _vec_cost; // Objective function +}; + +/// Generic Sparse container for LP (Linear Programming problems). +/// Embed : +/// - Constraints (coefficients, Sign, objective value), +/// - Bounds over parameter (<=, =, >=). +/// Implementation differ from LP_Constraints, here constraints are +/// stored as a Sparse matrix. +/// +struct LP_Constraints_Sparse +{ + LP_Constraints_Sparse() { + _bminimize = false; + } + + // Variable part + int _nbParams; // The number of parameter/variable in constraint. + std::vector< std::pair > _vec_bounds; // parameter/variable bounds. + + // Constraint part + sRMat _constraintMat; // Constraint under Matrix form. + Vec _Cst_objective; // Constraint objective value. + std::vector _vec_sign; // Constraint sign. + + bool _bminimize; // minimize is true or maximize is false. + std::vector _vec_cost; // Objective function +}; + +/// Generic LP solver (Linear Programming) +/// It's an interface to setup constraint and objective of a Linear Program. +/// Embed constraint setup, problem solving, and parameters getter. +class LP_Solver +{ +public: + + LP_Solver(int nbParams):_nbParams(nbParams){}; + + /// Setup constraint for the given library. + virtual bool setup(const LP_Constraints & constraints) = 0; + virtual bool setup(const LP_Constraints_Sparse & constraints) = 0; + + /// Setup the feasibility and found the solution that best fit the constraint. + virtual bool solve() = 0; + + /// Get back solution. Call it after solve. + virtual bool getSolution(std::vector & estimatedParams) = 0; + +protected : + int _nbParams; // The number of parameter considered in constraint formulation. +}; + +} // namespace linearProgramming +} // namespace openMVG + + +#endif // OPENMVG_LINEAR_PROGRAMMING_INTERFACE_H_ diff --git a/src/openMVG/linearProgramming/linearProgrammingMOSEK.cpp b/src/openMVG/linearProgramming/linearProgrammingMOSEK.cpp new file mode 100644 index 0000000000..7a2225fe6a --- /dev/null +++ b/src/openMVG/linearProgramming/linearProgrammingMOSEK.cpp @@ -0,0 +1,443 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifdef OPENMVG_HAVE_MOSEK + +#include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" +#include + +namespace openMVG { +namespace linearProgramming { + +using namespace std; + +// This function prints log output from MOSEK to the terminal. +static void MSKAPI printstr(void *handle, + char str[]) +{ + std::cout << str; +} + +MSKenv_t env = NULL; + +MOSEK_SolveWrapper::MOSEK_SolveWrapper(int nbParams) : LP_Solver(nbParams) +{ + task = NULL; + //env = NULL; + + //-- Initialize MOSEK framework + + // Create the mosek environment. + /*MSKrescodee r = MSK_makeenv(&env,NULL,NULL,NULL,NULL); + if ( r!=MSK_RES_OK ) { + std::cerr << "Cannot create the MOSEK environment" << std::endl; + }*/ + if (env == NULL) + { + MSKrescodee r = MSK_makeenv(&env,NULL,NULL,NULL,NULL); + + // Directs the env log stream to the 'printstr' function. + if ( r==MSK_RES_OK ) + MSK_linkfunctoenvstream(env,MSK_STREAM_LOG,NULL,printstr); + + // Initialize the environment. + if ( r==MSK_RES_OK ) + r = MSK_initenv(env); + else { + std::cerr << "Cannot create the MOSEK environment" << std::endl; + } + } + +} + +MOSEK_SolveWrapper::~MOSEK_SolveWrapper() +{ + // Memory cleaning. + MSK_deletetask(&task); + //MSK_deleteenv(&env); +} + + +inline MSKboundkey_enum convertSign(LP_Constraints::eLP_SIGN sign) { + + switch(sign) { + case LP_Constraints::LP_LESS_OR_EQUAL: // = 1, // cst (<=) VAL + return MSK_BK_UP; + case LP_Constraints::LP_GREATER_OR_EQUAL: // = 2, // cst (>=) VAL + return MSK_BK_LO; + case LP_Constraints::LP_EQUAL: // = 3, // cst (=) VAL + return MSK_BK_FX; + case LP_Constraints::LP_FREE: + return MSK_BK_FR; + default: + std::cerr << "Error unknow constraint sign : " << sign << "\n"; + } +} + +bool MOSEK_SolveWrapper::setup(const LP_Constraints & cstraints) //cstraints <-> constraints +{ + assert(_nbParams == cstraints._nbParams); + + bool bOk = true; + MSK_deletetask(&task); + + int NUMVAR = cstraints._constraintMat.cols(); + int NUMCON = cstraints._constraintMat.rows(); + int NUMANZ = cstraints._constraintMat.cols() * cstraints._constraintMat.rows(); //DENSE MATRIX + + MSKrescodee r = MSK_RES_OK; + if ( r==MSK_RES_OK ) + { + // Create the optimization task. + r = MSK_maketask(env,NUMCON,NUMVAR,&task); + + // Directs the log task stream to the 'printstr' function. + if ( r==MSK_RES_OK ) + MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr); + + // Give MOSEK an estimate of the size of the input data. + //This is done to increase the speed of inputting data. + // However, it is optional. + if (r == MSK_RES_OK) + r = MSK_putmaxnumvar(task,NUMVAR); + + if (r == MSK_RES_OK) + r = MSK_putmaxnumcon(task,NUMCON); + + if (r == MSK_RES_OK) + r = MSK_putmaxnumanz(task,NUMANZ); + + // Append 'NUMCON' empty constraints. The constraints will initially have no bounds. + if ( r == MSK_RES_OK ) + r = MSK_append(task,MSK_ACC_CON,NUMCON); + + // Append 'NUMVAR' variables. The variables will initially be fixed at zero (x=0). + if ( r == MSK_RES_OK ) + r = MSK_append(task,MSK_ACC_VAR,NUMVAR); + } + + this->_nbParams = NUMVAR; + + if (cstraints._bminimize) { + r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE); + } + else { + r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MAXIMIZE); + } + + // Optionally add a constant term to the objective. + if ( r ==MSK_RES_OK ) + r = MSK_putcfix(task,0.0); + + // Add the objective function if any + if (!cstraints._vec_cost.empty()) { + // Set objective + for (size_t i = 0; i < cstraints._vec_cost.size(); ++i) + MSK_putcj(task, i, cstraints._vec_cost[i]); + } + + //Add constraint row by row + const Mat & A = cstraints._constraintMat; + + for (int i=0; i vec_colno(A.cols(), 0); + for (int ii=0; ii constraints +{ + assert(_nbParams == cstraints._nbParams); + + bool bOk = true; + MSK_deletetask(&task); + + int NUMVAR = this->_nbParams; + int NUMCON = cstraints._constraintMat.rows(); + int NUMANZ = cstraints._constraintMat.nonZeros(); + + MSKrescodee r = MSK_RES_OK; + if ( r==MSK_RES_OK ) + { + // Directs the log task stream to the 'printstr' function. + if ( r==MSK_RES_OK ) + MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr); + + // Create the optimization task. + r = MSK_maketask(env,NUMCON,NUMVAR,&task); + + // Give MOSEK an estimate of the size of the input data. + //This is done to increase the speed of inputting data. + // However, it is optional. + if (r == MSK_RES_OK) + r = MSK_putmaxnumvar(task,NUMVAR); + + if (r == MSK_RES_OK) + r = MSK_putmaxnumcon(task,NUMCON); + + if (r == MSK_RES_OK) + r = MSK_putmaxnumanz(task,NUMANZ); + + // Append 'NUMCON' empty constraints. The constraints will initially have no bounds. + if ( r == MSK_RES_OK ) + r = MSK_append(task,MSK_ACC_CON,NUMCON); + + // Append 'NUMVAR' variables. The variables will initially be fixed at zero (x=0). + if ( r == MSK_RES_OK ) + r = MSK_append(task,MSK_ACC_VAR,NUMVAR); + } + + this->_nbParams = NUMVAR; + + if (cstraints._bminimize) { + r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE); + } + else { + r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MAXIMIZE); + } + + // Optionally add a constant term to the objective. + if ( r ==MSK_RES_OK ) + r = MSK_putcfix(task,0.0); + + // Add the objective function if any + if (!cstraints._vec_cost.empty()) { + // Set objective + for (size_t i = 0; i < cstraints._vec_cost.size(); ++i) + r = MSK_putcj(task, i, cstraints._vec_cost[i]); + } + + //Add constraint row by row + const sRMat & A = cstraints._constraintMat; + std::vector vec_colno; + std::vector vec_value; + for (int i=0; i & estimatedParams) +{ + bool bRet = false; + MSKsolstae solsta; + MSK_getsolutionstatus (task, + MSK_SOL_BAS, + NULL, + &solsta); + switch(solsta) + { + case MSK_SOL_STA_OPTIMAL: + case MSK_SOL_STA_NEAR_OPTIMAL: + MSK_getsolutionslice(task, + MSK_SOL_BAS, // Request the basic solution. + MSK_SOL_ITEM_XX,// Which part of solution. + 0, // Index of first variable. + estimatedParams.size(), // Index of last variable+1. + &estimatedParams[0]); + bRet = true; + //MSK_writedata(task,"taskdump.opf"); + + /*printf("Optimal primal solution\n"); + for(size_t j=0; j + +namespace openMVG { +namespace linearProgramming { + +/// MOSEK wrapper for the LP_Solver +class MOSEK_SolveWrapper : public LP_Solver +{ +public : + MOSEK_SolveWrapper(int nbParams); + + ~MOSEK_SolveWrapper(); + + //-- + // Inherited functions : + //-- + + bool setup(const LP_Constraints & constraints); + bool setup(const LP_Constraints_Sparse & constraints); + + bool solve(); + + bool getSolution(std::vector & estimatedParams); + +private : + //MSKenv_t env; + MSKtask_t task; // Solver object. +}; + + +} // namespace linearProgramming +} // namespace openMVG + + +#endif // MIMATTE_LINEAR_PROGRAMMING_INTERFACE_MOSEK_H_ + +#endif // OPENMVG_HAVE_MOSEK diff --git a/src/openMVG/linearProgramming/linearProgrammingOSI_X.hpp b/src/openMVG/linearProgramming/linearProgrammingOSI_X.hpp new file mode 100644 index 0000000000..70ec099448 --- /dev/null +++ b/src/openMVG/linearProgramming/linearProgrammingOSI_X.hpp @@ -0,0 +1,345 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef MIMATTE_LINEAR_PROGRAMMING_INTERFACE_OSICLP_H_ +#define MIMATTE_LINEAR_PROGRAMMING_INTERFACE_OSICLP_H_ + +#include "OsiClpSolverInterface.hpp" +#ifdef OPENMVG_HAVE_MOSEK +#include "OsiMskSolverInterface.hpp" +#endif + +#include "openMVG/numeric/numeric.h" +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" + +#include "CoinPackedMatrix.hpp" +#include "CoinPackedVector.hpp" + +#include + +/* Constraint type codes (internal) */ +#define ROWTYPE_EMPTY 0 +#define ROWTYPE_LE 1 +#define ROWTYPE_GE 2 +#define ROWTYPE_EQ 3 +#define ROWTYPE_CONSTRAINT ROWTYPE_EQ /* This is the mask for modes */ +#define ROWTYPE_OF 4 +#define ROWTYPE_INACTIVE 8 +#define ROWTYPE_RELAX 16 +#define ROWTYPE_GUB 32 +#define ROWTYPE_OFMAX (ROWTYPE_OF + ROWTYPE_GE) +#define ROWTYPE_OFMIN (ROWTYPE_OF + ROWTYPE_LE) +#define ROWTYPE_CHSIGN ROWTYPE_GE + +/* Public constraint codes */ +#define FR ROWTYPE_EMPTY +#define LE ROWTYPE_LE +#define GE ROWTYPE_GE +#define EQ ROWTYPE_EQ +#define OF ROWTYPE_OF + +namespace openMVG { +namespace linearProgramming { + + +/// OSI_X wrapper for the LP_Solver +template +class OSI_X_SolverWrapper : public LP_Solver +{ +public : + OSI_X_SolverWrapper(int nbParams); + + ~OSI_X_SolverWrapper(); + + //-- + // Inherited functions : + //-- + + bool setup(const LP_Constraints & constraints); + bool setup(const LP_Constraints_Sparse & constraints); + + bool solve(); + + bool getSolution(std::vector & estimatedParams); + +private : + SOLVERINTERFACE *si; +}; + + +typedef OSI_X_SolverWrapper OSI_CLP_SolverWrapper; +#ifdef OPENMVG_HAVE_MOSEK +typedef OSI_X_SolverWrapper OSI_MOSEK_SolverWrapper; +#endif // OPENMVG_HAVE_MOSEK + + + +template +OSI_X_SolverWrapper::OSI_X_SolverWrapper(int nbParams) : LP_Solver(nbParams) +{ + si = new SOLVERINTERFACE; +} + +template +OSI_X_SolverWrapper::~OSI_X_SolverWrapper() +{ + // Memory cleaning. + if ( si != NULL ) + { + delete si; + si = NULL; + } +} + +template +bool OSI_X_SolverWrapper::setup(const LP_Constraints & cstraints) //cstraints <-> constraints +{ + bool bOk = true; + if ( si == NULL ) + { + return false; + } + assert(_nbParams == cstraints._nbParams); + + + int NUMVAR = cstraints._constraintMat.cols(); + int NUMCON = cstraints._constraintMat.rows(); + int NUMANZ = cstraints._constraintMat.cols() * cstraints._constraintMat.rows(); //DENSE MATRIX + + std::vector col_lb(NUMVAR);//the column lower bounds + std::vector col_ub(NUMVAR);//the column upper bounds + + this->_nbParams = NUMVAR; + + if (cstraints._bminimize) + { + si->setObjSense( 1 ); + } + else + { + si->setObjSense( -1 ); + } + + const Mat & A = cstraints._constraintMat; + + //Equality constraint will be handked by two constraintsdue to the API limitation. + size_t nbLine = A.rows() + std::count(cstraints._vec_sign.begin(), cstraints._vec_sign.end(), EQ); + + std::vector row_lb(nbLine);//the row lower bounds + std::vector row_ub(nbLine);//the row upper bounds + + CoinPackedMatrix * matrix = new CoinPackedMatrix(false,0,0); + matrix->setDimensions(0, NUMVAR); + + + //-- Add row-wise constraint + size_t indexRow = 0; + for (int i=0; i < A.rows(); ++i) + { + Vec temp = A.row(i); + + CoinPackedVector row; + if ( cstraints._vec_sign[i] == EQ || cstraints._vec_sign[i] == LE ) + { + int coef = 1; + for ( int j = 0; j < A.cols() ; j++ ) + { + row.insert(j, coef * temp.data()[j]); + } + row_lb[indexRow] = -1.0 * si->getInfinity(); + row_ub[indexRow] = coef * cstraints._Cst_objective(i); + matrix->appendRow(row); + indexRow++; + } + if ( cstraints._vec_sign[i] == EQ || cstraints._vec_sign[i] == GE ) + { + int coef = -1; + for ( int j = 0; j < A.cols() ; j++ ) + { + row.insert(j, coef * temp.data()[j]); + } + row_lb[indexRow] = -1.0 * si->getInfinity(); + row_ub[indexRow] = coef * cstraints._Cst_objective(i); + matrix->appendRow(row); + indexRow++; + } + } + + //-- Setup bounds + if (cstraints._vec_bounds.size() == 1) + { + // Setup the same bound for all the parameter + for (int i=0; i < this->_nbParams; ++i) + { + col_lb[i] = cstraints._vec_bounds[0].first; + col_ub[i] = cstraints._vec_bounds[0].second; + } + } + else + { + + for (int i=0; i < this->_nbParams; ++i) + { + col_lb[i] = cstraints._vec_bounds[i].first; + col_ub[i] = cstraints._vec_bounds[i].second; + } + } + + si->loadProblem(*matrix, &col_lb[0], &col_ub[0], cstraints._vec_cost.empty() ? NULL : &cstraints._vec_cost[0], &row_lb[0], &row_ub[0] ); + + delete matrix; + + return bOk; +} + +template +bool OSI_X_SolverWrapper::setup(const LP_Constraints_Sparse & cstraints) //cstraints <-> constraints +{ + bool bOk = true; + if ( si == NULL ) + { + return false; + } + assert(_nbParams == cstraints._nbParams); + + + int NUMVAR = cstraints._constraintMat.cols(); + int NUMCON = cstraints._constraintMat.rows(); + int NUMANZ = cstraints._constraintMat.cols() * cstraints._constraintMat.rows(); //DENSE MATRIX + std::vector col_lb(NUMVAR);//the column lower bounds + std::vector col_ub(NUMVAR);//the column upper bounds + + this->_nbParams = NUMVAR; + + if (cstraints._bminimize) + { + si->setObjSense( 1 ); + } + else + { + si->setObjSense( -1 ); + } + + const sRMat & A = cstraints._constraintMat; + + //Equality constraint will be handked by two constraintsdue to the API limitation. + size_t nbLine = A.rows() + std::count(cstraints._vec_sign.begin(), cstraints._vec_sign.end(), EQ); + + std::vector row_lb(nbLine);//the row lower bounds + std::vector row_ub(nbLine);//the row upper bounds + + CoinPackedMatrix * matrix = new CoinPackedMatrix(false,0,0); + matrix->setDimensions(0, NUMVAR); + + //-- Add row-wise constraint + size_t rowindex = 0; + for (int i=0; i < A.rows(); ++i) + { + std::vector vec_colno; + std::vector vec_value; + for (sRMat::InnerIterator it(A,i); it; ++it) + { + vec_colno.push_back(it.col()); + vec_value.push_back(it.value()); + } + + + if ( cstraints._vec_sign[i] == EQ || cstraints._vec_sign[i] == LE ) + { + int coef = 1; + row_lb[rowindex] = -1.0 * si->getInfinity(); + row_ub[rowindex] = coef * cstraints._Cst_objective(i);; + matrix->appendRow( vec_colno.size(), + &vec_colno[0], + &vec_value[0] ); + rowindex++; + } + + if ( cstraints._vec_sign[i] == EQ || cstraints._vec_sign[i] == GE ) + { + int coef = -1; + for ( std::vector::iterator iter_val = vec_value.begin(); + iter_val != vec_value.end(); + iter_val++) + { + *iter_val *= coef; + } + row_lb[rowindex] = -1.0 * si->getInfinity(); + row_ub[rowindex] = coef * cstraints._Cst_objective(i);; + matrix->appendRow( vec_colno.size(), + &vec_colno[0], + &vec_value[0] ); + rowindex++; + } + } + + //-- Setup bounds + if (cstraints._vec_bounds.size() == 1) + { + // Setup the same bound for all the parameter + for (int i=0; i < this->_nbParams; ++i) + { + col_lb[i] = cstraints._vec_bounds[0].first; + col_ub[i] = cstraints._vec_bounds[0].second; + } + } + else { + // Set the required bound per constraint + for (int i=0; i < this->_nbParams; ++i) + { + col_lb[i] = cstraints._vec_bounds[i].first; + col_ub[i] = cstraints._vec_bounds[i].second; + } + } + + si->loadProblem( + *matrix, + &col_lb[0], + &col_ub[0], + cstraints._vec_cost.empty() ? NULL : &cstraints._vec_cost[0], + &row_lb[0], + &row_ub[0]); + + delete matrix; + + return bOk; +} + +template +bool OSI_X_SolverWrapper::solve() +{ + //-- Compute solution + if ( si != NULL ) + { + si->initialSolve(); + return si->isProvenOptimal(); + } + return false; +} + +template +bool OSI_X_SolverWrapper::getSolution(std::vector & estimatedParams) +{ + if ( si != NULL ) + { + int n = si->getNumCols(); + const double *solution; + solution = si->getColSolution(); + for ( int i = 0; i < n ; i++ ) + { + estimatedParams[i] = solution[i]; + } + return true; + } + return false; +} + +} // namespace linearProgramming +} // namespace openMVG + + +#endif // MIMATTE_LINEAR_PROGRAMMING_INTERFACE_OSICLP_H_ + diff --git a/src/openMVG/linearProgramming/linearProgramming_test.cpp b/src/openMVG/linearProgramming/linearProgramming_test.cpp new file mode 100644 index 0000000000..9704e12e91 --- /dev/null +++ b/src/openMVG/linearProgramming/linearProgramming_test.cpp @@ -0,0 +1,220 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + + +#include +#include +#include + +#include "testing/testing.h" +#include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" + + +using namespace openMVG; +using namespace openMVG::linearProgramming; + +// Setup : +// max(143x + 60y) +// s.t. +// 120x + 210y <= 15000 +// 110x + 30y <= 4000 +// x + y <= 75 +// x >= 0 +// y >= 0 +void BuildLinearProblem(LP_Constraints & cstraint) +{ + cstraint._nbParams = 2; + cstraint._bminimize = false; + + //Configure objective + cstraint._vec_cost.push_back(143); + cstraint._vec_cost.push_back(60); + + cstraint._constraintMat = Mat(5,2); + cstraint._constraintMat << + 120, 210, + 110, 30, + 1, 1, + 1, 0, + 0, 1; + + cstraint._Cst_objective = Vec(5); + cstraint._Cst_objective << 15000, 4000, 75, 0, 0; + + cstraint._vec_sign.resize(5); + std::fill_n(cstraint._vec_sign.begin(), 3, LP_Constraints::LP_LESS_OR_EQUAL); + std::fill_n(cstraint._vec_sign.begin()+3, 2, LP_Constraints::LP_GREATER_OR_EQUAL); + + cstraint._vec_bounds = std::vector< std::pair >(5); + fill(cstraint._vec_bounds.begin(),cstraint._vec_bounds.end(), + std::make_pair((double)-1e+30, (double)1e+30)); +} + +#ifdef OPENMVG_HAVE_MOSEK +// LP_Solve website example solving with the HighLevelFramework +TEST(linearProgramming, MOSEK_dense_sample) { + + LP_Constraints cstraint; + BuildLinearProblem(cstraint); + + //Solve + std::vector vec_solution(2); + MOSEK_SolveWrapper solver(2); + solver.setup(cstraint); + + EXPECT_TRUE(solver.solve()); + solver.getSolution(vec_solution); + + EXPECT_NEAR( 21.875000, vec_solution[0], 1e-6); + EXPECT_NEAR( 53.125000, vec_solution[1], 1e-6); + + std::cout << "Solution : " << vec_solution[0] << " " << vec_solution[1] << std::endl; +} +#endif // OPENMVG_HAVE_MOSEK + +TEST(linearProgramming, osiclp_dense_sample) { + + LP_Constraints cstraint; + BuildLinearProblem(cstraint); + + //Solve + std::vector vec_solution(2); + OSI_CLP_SolverWrapper solver(2); + solver.setup(cstraint); + + EXPECT_TRUE(solver.solve()); + solver.getSolution(vec_solution); + + EXPECT_NEAR( 21.875000, vec_solution[0], 1e-6); + EXPECT_NEAR( 53.125000, vec_solution[1], 1e-6); +} + +// Setup example from MOSEK API +// maximize : +// 3 x0 + 1 x1 + 5 x2 + 1 x3 +// subject to +// 3 x0 + 1 x1 + 2 x2 = 30 +// 2 x0 + 1 x1 + 3 x2 + 1 x3 >= 15 +// 2 w1 + 3 x3 <= 25 +// bounds +// 0 <= x0, x2, x3 < infinity +// 0 <= x1 <= 10 +void BuildSparseLinearProblem(LP_Constraints_Sparse & cstraint) +{ + // Number of variable we are looking for + cstraint._nbParams = 4; // {x0, x1, x2, x3} + + // Constraint coefficient + sRMat & A = cstraint._constraintMat; + A.resize(3,4); + A.coeffRef(0,0) = 3; + A.coeffRef(0,1) = 1; + A.coeffRef(0,2) = 2; + + A.coeffRef(1,0) = 2; + A.coeffRef(1,1) = 1; + A.coeffRef(1,2) = 3; + A.coeffRef(1,3) = 1; + + A.coeffRef(2,1) = 2; + A.coeffRef(2,3) = 3; + + // Constraint objective + Vec & C = cstraint._Cst_objective; + C.resize(3, 1); + C[0] = 30; + C[1] = 15; + C[2] = 25; + + // Constraint sign + std::vector & vec_sign = cstraint._vec_sign; + vec_sign.resize(3); + vec_sign[0] = LP_Constraints::LP_EQUAL; + vec_sign[1] = LP_Constraints::LP_GREATER_OR_EQUAL; + vec_sign[2] = LP_Constraints::LP_LESS_OR_EQUAL; + + // Variable bounds + cstraint._vec_bounds = std::vector< std::pair >(4); + fill(cstraint._vec_bounds.begin(),cstraint._vec_bounds.end(), + std::make_pair(0.0, (double)1e+30)); + cstraint._vec_bounds[1].second = 10; + + // Objective to maximize + cstraint._bminimize = false; + cstraint._vec_cost.resize(4); + cstraint._vec_cost[0] = 3; + cstraint._vec_cost[1] = 1; + cstraint._vec_cost[2] = 5; + cstraint._vec_cost[3] = 1; +} + +#ifdef OPENMVG_HAVE_MOSEK +// Unit test on mosek Sparse constraint +TEST(linearProgramming, mosek_sparse_sample) { + + LP_Constraints_Sparse cstraint; + BuildSparseLinearProblem(cstraint); + + //Solve + std::vector vec_solution(4); + MOSEK_SolveWrapper solver(4); + solver.setup(cstraint); + + EXPECT_TRUE(solver.solve()); + solver.getSolution(vec_solution); + + EXPECT_NEAR( 0.00, vec_solution[0], 1e-2); + EXPECT_NEAR( 0.00, vec_solution[1], 1e-2); + EXPECT_NEAR( 15, vec_solution[2], 1e-2); + EXPECT_NEAR( 8.33, vec_solution[3], 1e-2); +} +#endif // #ifdef OPENMVG_HAVE_MOSEK + +TEST(linearProgramming, osiclp_sparse_sample) { + + LP_Constraints_Sparse cstraint; + BuildSparseLinearProblem(cstraint); + + //Solve + std::vector vec_solution(4); + OSI_CLP_SolverWrapper solver(4); + solver.setup(cstraint); + + EXPECT_TRUE(solver.solve()); + solver.getSolution(vec_solution); + + EXPECT_NEAR( 0.00, vec_solution[0], 1e-2); + EXPECT_NEAR( 0.00, vec_solution[1], 1e-2); + EXPECT_NEAR( 15, vec_solution[2], 1e-2); + EXPECT_NEAR( 8.33, vec_solution[3], 1e-2); +} + +#ifdef OPENMVG_HAVE_MOSEK +TEST(linearProgramming, osi_mosek_sparse_sample) { + + LP_Constraints_Sparse cstraint; + BuildSparseLinearProblem(cstraint); + + //Solve + std::vector vec_solution(4); + OSI_MOSEK_SolverWrapper solver(4); + solver.setup(cstraint); + + EXPECT_TRUE(solver.solve()); + solver.getSolution(vec_solution); + + EXPECT_NEAR( 0.00, vec_solution[0], 1e-2); + EXPECT_NEAR( 0.00, vec_solution[1], 1e-2); + EXPECT_NEAR( 15, vec_solution[2], 1e-2); + EXPECT_NEAR( 8.33, vec_solution[3], 1e-2); +} +#endif // #ifdef OPENMVG_HAVE_MOSEK + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/src/openMVG/numeric/numeric.h b/src/openMVG/numeric/numeric.h index dc0c017a0b..e3b82921a9 100644 --- a/src/openMVG/numeric/numeric.h +++ b/src/openMVG/numeric/numeric.h @@ -37,9 +37,11 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -96,6 +98,10 @@ namespace openMVG { typedef Eigen::Matrix MatX9; + //-- Sparse Matrix (Column major, and row major) + typedef Eigen::SparseMatrix sMat; + typedef Eigen::SparseMatrix sRMat; + //-------------- //-- Function -- //-------------- From 675ec0798351726035fead2c1bb37d66c5fe1cd3 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 29 Dec 2013 19:19:27 +0100 Subject: [PATCH 04/68] Add initial revision of the l_infinity solvers for computer vision problems. #47 --- src/openMVG/linearProgramming/CMakeLists.txt | 1 + .../lInfinitycomputervision/CMakeLists.txt | 34 +++ .../lInfinitycomputervision/resection.hpp | 192 ++++++++++++ .../resection_L_Infty_robust_test.cpp | 123 ++++++++ .../resection_L_Infty_test.cpp | 152 +++++++++ .../resection_kernel.cpp | 68 +++++ .../resection_kernel.hpp | 50 +++ .../translationAndStructureFrom_xi_Ri.hpp | 191 ++++++++++++ ...ranslationAndStructureFrom_xi_Ri_noise.hpp | 221 ++++++++++++++ ...ationAndStructure_L_Infty_outlier_test.cpp | 258 ++++++++++++++++ .../translationAndStructure_L_Infty_test.cpp | 289 ++++++++++++++++++ .../lInfinitycomputervision/triangulation.hpp | 126 ++++++++ .../triangulation_L_Infty_test.cpp | 149 +++++++++ 13 files changed, 1854 insertions(+) create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/resection.hpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_robust_test.cpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_test.cpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.cpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri.hpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri_noise.hpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_outlier_test.cpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_test.cpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/triangulation.hpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/triangulation_L_Infty_test.cpp diff --git a/src/openMVG/linearProgramming/CMakeLists.txt b/src/openMVG/linearProgramming/CMakeLists.txt index 488595a97e..f84aff85b1 100644 --- a/src/openMVG/linearProgramming/CMakeLists.txt +++ b/src/openMVG/linearProgramming/CMakeLists.txt @@ -32,3 +32,4 @@ ENDIF (MOSEK_FOUND) UNIT_TEST(openMVG_linearProgramming linearProgramming "openMVG_linearProgramming") +ADD_SUBDIRECTORY(lInfinitycomputervision) diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt b/src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt new file mode 100644 index 0000000000..ea85a2e211 --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt @@ -0,0 +1,34 @@ + +FILE( + GLOB + lInftycomputervision_headers + *.hpp +) + +FILE( + GLOB + lInftycomputervision_cpp + *.cpp +) +SET_SOURCE_FILES_PROPERTIES(${lInftycomputervision_headers} PROPERTIES LANGUAGE CXX) + +FILE(GLOB_RECURSE REMOVEFILESUNITTEST *_test.cpp) + +# Remove the future main files +LIST(REMOVE_ITEM lInftycomputervision_cpp ${REMOVEFILESUNITTEST}) + +ADD_LIBRARY(openMVG_lInftyComputerVision ${lInftycomputervision_headers} ${lInftycomputervision_cpp}) + +# Triangulation +UNIT_TEST(openMVG_lInftyComputerVision triangulation_L_Infty "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") + +# Pose/Resection estimation +UNIT_TEST(openMVG_lInftyComputerVision resection_L_Infty + "openMVG_multiview_test_data;openMVG_multiview;openMVG_linearProgramming;openMVG_lInftyComputerVision") +UNIT_TEST(openMVG_lInftyComputerVision resection_L_Infty_robust + "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") + +# Translation and Structure +UNIT_TEST(openMVG_lInfinityComputerVision translationAndStructure_L_Infty "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") +# Translation and Structure with noise handling +UNIT_TEST(openMVG_lInfinityComputerVision translationAndStructure_L_Infty_outlier "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection.hpp b/src/openMVG/linearProgramming/lInfinitycomputervision/resection.hpp new file mode 100644 index 0000000000..a637ec56f1 --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/resection.hpp @@ -0,0 +1,192 @@ +// Copyright (c) 2012 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_LINFINITY_COMPUTER_VISION_RESECTION_H_ +#define OPENMVG_LINFINITY_COMPUTER_VISION_RESECTION_H_ + +#include "openMVG/numeric/numeric.h" +#include "openMVG/linearProgramming/linearProgrammingInterface.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 openMVG { +namespace lInfinitycomputervision { + +using namespace linearProgramming; + +//-- Camera Resection +// - Estimation of [Ri|Ti] from xij and Xi +// [1] -> 5.4 Camera Resectioning +// +// The projection is parametrized as : +// | p11 p12 p13 p14 | +// P= | p21 p22 p23 p24 | +// | p31 p32 p33 1 | +// +// - 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 +// +static +void EncodeResection(const Mat2X & Pt2D, + const Mat3X & Pt3D, + double gamma, // Start upper bound + sRMat & A, Vec & C) +{ + // Build Constraint matrix. + + const int Nobs = Pt2D.cols(); + + assert(Pt2D.cols() == Pt3D.cols()); + assert(Pt2D.cols() >= 6 && "The problem requires at least 6 points"); + + A.resize(Nobs * 5, 11); + + C.resize(Nobs * 5, 1); + C.fill(0.0); + + for (int p=0; p 0 + // - R^3 X - t < 0 => // - R^3 X < 1.0 because t3 = 1.0 by default + int cpti = 8; + int cptj = 5 * p; + A.coeffRef(cptj, cpti) = -pt3d(0); + A.coeffRef(cptj, cpti+1) = -pt3d(1); + A.coeffRef(cptj, cpti+2) = -pt3d(2); + C(cptj) = 1.0; + + // First constraint <= s + // R1 * X + t1 + R3 * X *(-xij -s) <= xij t3 + s t3 + cpti = 0; + cptj += 1; + A.coeffRef(cptj, cpti) = pt3d(0); + A.coeffRef(cptj, cpti+1) = pt3d(1); + A.coeffRef(cptj, cpti+2) = pt3d(2); + A.coeffRef(cptj, cpti+3) = 1.0; + cpti = 4; + A.coeffRef(cptj+1, cpti) = pt3d(0); + A.coeffRef(cptj+1, cpti+1) = pt3d(1); + A.coeffRef(cptj+1, cpti+2) = pt3d(2); + A.coeffRef(cptj+1, cpti+3) = 1.0; + + cpti = 8; + Mat temp; + temp = Vec2((-pt2d).array() - gamma) * pt3d.transpose(); + A.coeffRef(cptj, cpti) = temp(0,0); + A.coeffRef(cptj, cpti+1) = temp(0,1); + A.coeffRef(cptj, cpti+2) = temp(0,2); + + A.coeffRef(cptj+1, cpti) = temp(1,0); + A.coeffRef(cptj+1, cpti+1) = temp(1,1); + A.coeffRef(cptj+1, cpti+2) = temp(1,2); + + C(cptj) = gamma + pt2d(0); + C(cptj+1) = gamma + pt2d(1); + + + // Second constraint >= s + // -R1 * X - t1 + R3 * X *(xij -s) <= - xij t3 + s t3 + cpti = 0; + cptj += 2; + A.coeffRef(cptj, cpti) = - pt3d(0); + A.coeffRef(cptj, cpti+1) = - pt3d(1); + A.coeffRef(cptj, cpti+2) = - pt3d(2); + A.coeffRef(cptj, cpti+3) = - 1.0; + cpti = 4; + A.coeffRef(cptj+1, cpti) = - pt3d(0); + A.coeffRef(cptj+1, cpti+1) = - pt3d(1); + A.coeffRef(cptj+1, cpti+2) = - pt3d(2); + A.coeffRef(cptj+1, cpti+3) = - 1.0; + + cpti = 8; + temp = Vec2(pt2d.array() - gamma) * pt3d.transpose(); + A.coeffRef(cptj, cpti) = temp(0,0); + A.coeffRef(cptj, cpti+1) = temp(0,1); + A.coeffRef(cptj, cpti+2) = temp(0,2); + + A.coeffRef(cptj+1, cpti) = temp(1,0); + A.coeffRef(cptj+1, cpti+1) = temp(1,1); + A.coeffRef(cptj+1, cpti+2) = temp(1,2); + + C(cptj) = gamma - pt2d(0); + C(cptj+1) = gamma - pt2d(1); + } +} + +/// Kernel that set Linear constraints for the +/// - Translation Registration and Structure Problem. +/// Designed to be used with bisectionLP and LP_Solver interface. +/// +/// Implementation of camera Resection +/// - Estimation of [Ri|Ti] from xij and Xi +/// [1] -> 5.4 Camera Resectioning +/// +struct Resection_L1_ConstraintBuilder +{ + Resection_L1_ConstraintBuilder( + const Mat2X & Pt2D, + const Mat3X & Pt3D) + { + _2DPt = Pt2D; + _3DPt = Pt3D; + } + + /// Setup constraints for the Resection problem, + /// in the LP_Constraints object. + bool Build(double gamma, LP_Constraints_Sparse & constraint) + { + EncodeResection(_2DPt, _3DPt, + gamma, + constraint._constraintMat, + constraint._Cst_objective); + + //-- Setup additional information about the Linear Program constraint + // We look for: + // P = [P00 P01 P02 P03; + // P10 P11 P12 P13; + // P20 P21 P22 1.0]; + const int NParams = 4 * 2 + 3; + + constraint._nbParams = NParams; + constraint._vec_bounds = std::vector< std::pair >(1); + fill(constraint._vec_bounds.begin(),constraint._vec_bounds.end(), + std::make_pair((double)-1e+30, (double)1e+30)); // lp_solve => getInfinite => DEF_INFINITE + // Constraint sign are all LESS or equal (<=) + constraint._vec_sign.resize(constraint._constraintMat.rows()); + fill(constraint._vec_sign.begin(), constraint._vec_sign.end(), + LP_Constraints::LP_LESS_OR_EQUAL); + + return true; + } + + Mat2X _2DPt; + Mat3X _3DPt; +}; + +} // namespace lInfinitycomputervision +} // namespace openMVG + +#endif // OPENMVG_LINFINITY_COMPUTER_VISION_RESECTION_H_ diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_robust_test.cpp b/src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_robust_test.cpp new file mode 100644 index 0000000000..247657a18f --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_robust_test.cpp @@ -0,0 +1,123 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "openMVG/multiview/test_data_sets.hpp" +#include "CppUnitLite/TestHarness.h" +#include "testing/testing.h" + +#include "openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp" +#include "openMVG/robust_estimation/robust_estimator_MaxConsensus.hpp" +#include "openMVG/robust_estimation/score_evaluator.hpp" +#include "openMVG/multiview/projection.hpp" + +#include +#include + +using namespace openMVG; +using namespace openMVG::robust; + +TEST(Resection_L_Infinity, Robust_OutlierFree) { + + const int nViews = 3; + const int nbPoints = 10; + const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, + nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + + //-- Modify a dataset (set to 0 and parse new value) (Assert good values) + NViewDataSet d2 = d; + + const int nResectionCameraIndex = 2; + //-- Set to 0 the future computed data to be sure of computation results : + d2._R[nResectionCameraIndex] = Mat3::Zero(); + d2._t[nResectionCameraIndex] = Vec3::Zero(); + + // Solve the problem and check that fitted value are good enough + { + typedef lInfinitycomputervision::kernel::l1PoseResectionKernel KernelType; + const Mat & pt2D = d2._x[nResectionCameraIndex]; + const Mat & pt3D = d2._X; + KernelType kernel(pt2D, pt3D); + ScorerEvaluator scorer(2*Square(0.6)); + Mat34 P = MaxConsensus(kernel, scorer, NULL, 128); + + // Check that Projection matrix is near to the GT : + Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array() + / d.P(nResectionCameraIndex).norm(); + Mat34 COMPUTED_ProjectionMatrix = P.array() / P.norm(); + + // Extract K[R|t] + Mat3 R,K; + Vec3 t; + KRt_From_P(P, &K, &R, &t); + + d2._R[nResectionCameraIndex] = R; + d2._t[nResectionCameraIndex] = t; + + //CHeck matrix to GT, and residual + EXPECT_NEAR( 0.0, FrobeniusDistance(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix), 1e-2 ); + Mat pt4D = VStack(pt3D, Mat(Vec::Ones(pt3D.cols()).transpose())); + EXPECT_NEAR( 0.0, RootMeanSquareError(pt2D, pt4D, COMPUTED_ProjectionMatrix), 1e-2); + } +} + +TEST(Resection_L_Infinity, Robust_OneOutlier) { + + const int nViews = 3; + const int nbPoints = 20; + const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, + nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + + d.ExportToPLY("test_Before_Infinity.ply"); + //-- Modify a dataset (set to 0 and parse new value) (Assert good values) + NViewDataSet d2 = d; + + const int nResectionCameraIndex = 2; + //-- Set to 0 the future computed data to be sure of computation results : + d2._R[nResectionCameraIndex] = Mat3::Zero(); + d2._t[nResectionCameraIndex] = Vec3::Zero(); + + // Set 20% of the 3D point as outlier + const int nbOutlier = nbPoints*0.2; + for (int i=0; i < nbOutlier; ++i) + { + d2._X.col(i)(0) += 120.0; + d2._X.col(i)(1) += -60.0; + d2._X.col(i)(2) += 80.0; + } + + // Solve the problem and check that fitted value are good enough + { + typedef lInfinitycomputervision::kernel::l1PoseResectionKernel KernelType; + const Mat & pt2D = d2._x[nResectionCameraIndex]; + const Mat & pt3D = d2._X; + KernelType kernel(pt2D, pt3D); + ScorerEvaluator scorer(Square(0.1)); //Highly intolerant for the test + Mat34 P = MaxConsensus(kernel, scorer, NULL, 128); + + // Check that Projection matrix is near to the GT : + Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array() + / d.P(nResectionCameraIndex).norm(); + Mat34 COMPUTED_ProjectionMatrix = P.array() / P.norm(); + + // Extract K[R|t] + Mat3 R,K; + Vec3 t; + KRt_From_P(P, &K, &R, &t); + + d2._R[nResectionCameraIndex] = R; + d2._t[nResectionCameraIndex] = t; + + //CHeck matrix to GT, and residual + EXPECT_NEAR( 0.0, FrobeniusDistance(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix), 1e-3 ); + Mat pt4D = VStack(pt3D, Mat(Vec::Ones(pt3D.cols()).transpose())); + EXPECT_NEAR( 0.0, RootMeanSquareError(pt2D, pt4D, COMPUTED_ProjectionMatrix), 1e-1); + } + d2.ExportToPLY("test_After_Infinity.ply"); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_test.cpp b/src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_test.cpp new file mode 100644 index 0000000000..31ebb5b7fb --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_test.cpp @@ -0,0 +1,152 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include +#include + +#include "openMVG/multiview/test_data_sets.hpp" +#include "openMVG/multiview/projection.hpp" +#include "CppUnitLite/TestHarness.h" +#include "testing/testing.h" + +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" +#include "openMVG/linearProgramming/bisectionLP.hpp" +#include "openMVG/linearProgramming/lInfinitycomputervision/resection.hpp" + +using namespace openMVG; + +using namespace linearProgramming; +using namespace lInfinitycomputervision; + +void translate(const Mat3X & X, const Vec3 & vecTranslation, + Mat3X * XPoints) { + XPoints->resize(X.rows(), X.cols()); + for (typename Mat3::Index i=0; icol(i) = X.col(i) + vecTranslation; + } +} + +TEST(Resection_L_Infinity, OSICLP) { + + const int nViews = 3; + const int nbPoints = 10; + const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, + nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + + d.ExportToPLY("test_Before_Infinity.ply"); + //-- Modify a dataset (set to 0 and parse new value) (Assert good values) + NViewDataSet d2 = d; + + const int nResectionCameraIndex = 2; + //-- Set to 0 the future computed data to be sure of computation results : + d2._R[nResectionCameraIndex] = Mat3::Zero(); + d2._t[nResectionCameraIndex] = Vec3::Zero(); + + // Solve the problem and check that fitted value are good enough + { + std::vector vec_solution(11); + + //-- Translate 3D point in order to have X0 = (0,0,0,1). + Vec3 vecTranslation = - d2._X.col(0); + Mat4 translationMatrix = Mat4::Identity(); + translationMatrix << 1, 0, 0, vecTranslation(0), + 0, 1, 0, vecTranslation(1), + 0, 0, 1, vecTranslation(2), + 0, 0, 0, 1; + Mat3X XPoints; + translate(d2._X, vecTranslation, &XPoints); + + OSI_CLP_SolverWrapper wrapperOSICLPSolver(vec_solution.size()); + Resection_L1_ConstraintBuilder cstBuilder(d2._x[nResectionCameraIndex], XPoints); + EXPECT_TRUE( + (BisectionLP( + wrapperOSICLPSolver, + cstBuilder, + &vec_solution, + 1.0, + 0.0)) + ); + + // Move computed value to dataset for residual estimation. + Mat34 P; + P << vec_solution[0], vec_solution[1], vec_solution[2], vec_solution[3], + vec_solution[4], vec_solution[5], vec_solution[6], vec_solution[7], + vec_solution[8], vec_solution[9], vec_solution[10], 1.0; + P = P * translationMatrix; + + // Check that Projection matrix is near to the GT : + Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array() + / d.P(nResectionCameraIndex).norm(); + Mat34 COMPUTED_ProjectionMatrix = P.array() / P.norm(); + EXPECT_MATRIX_NEAR(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix, 1e-4); + } + d2.ExportToPLY("test_After_Infinity.ply"); +} + +#ifdef OPENMVG_HAVE_MOSEK +TEST(Resection_L_Infinity, MOSEK) { + + const int nViews = 3; + const int nbPoints = 10; + const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, + nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + + d.ExportToPLY("test_Before_Infinity.ply"); + //-- Modify a dataset (set to 0 and parse new value) (Assert good values) + NViewDataSet d2 = d; + + const int nResectionCameraIndex = 2; + //-- Set to 0 the future computed data to be sure of computation results : + d2._R[nResectionCameraIndex] = Mat3::Zero(); + d2._t[nResectionCameraIndex] = Vec3::Zero(); + + // Solve the problem and check that fitted value are good enough + { + std::vector vec_solution(11); + + //-- Translate 3D point in order to have X0 = (0,0,0,1). + Vec3 vecTranslation = - d2._X.col(0); + Mat4 translationMatrix = Mat4::Identity(); + translationMatrix << 1, 0, 0, vecTranslation(0), + 0, 1, 0, vecTranslation(1), + 0, 0, 1, vecTranslation(2), + 0, 0, 0, 1; + Mat3X XPoints; + translate(d2._X, vecTranslation, &XPoints); + + MOSEK_SolveWrapper wrapperMosek(vec_solution.size()); + Resection_L1_ConstraintBuilder cstBuilder(d2._x[nResectionCameraIndex], XPoints); + EXPECT_TRUE( + (BisectionLP( + wrapperMosek, + cstBuilder, + &vec_solution, + 1.0, + 0.0)) + ); + + // Move computed value to dataset for residual estimation. + Mat34 P; + P << vec_solution[0], vec_solution[1], vec_solution[2], vec_solution[3], + vec_solution[4], vec_solution[5], vec_solution[6], vec_solution[7], + vec_solution[8], vec_solution[9], vec_solution[10], 1.0; + P = P * translationMatrix; + + // Check that Projection matrix is near to the GT : + Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array() + / d.P(nResectionCameraIndex).norm(); + Mat34 COMPUTED_ProjectionMatrix = P.array() / P.norm(); + EXPECT_MATRIX_NEAR(GT_ProjectionMatrix, COMPUTED_ProjectionMatrix, 1e-4); + } + d2.ExportToPLY("test_After_Infinity.ply"); +} +#endif // #ifdef OPENMVG_HAVE_MOSEK + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.cpp b/src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.cpp new file mode 100644 index 0000000000..47d3fa8ded --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.cpp @@ -0,0 +1,68 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#include "openMVG/linearProgramming/bisectionLP.hpp" + +#include "openMVG/linearProgramming/lInfinitycomputervision/resection.hpp" +#include "openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp" + +#include + +namespace openMVG { +namespace lInfinitycomputervision { +namespace kernel { + +using namespace std; + +void translate(const Mat3X & X, const Vec3 & vecTranslation, + Mat3X * XPoints) { + XPoints->resize(X.rows(), X.cols()); + for (typename Mat3::Index i=0; icol(i) = X.col(i) + vecTranslation; + } +} + +void l1SixPointResectionSolver::Solve(const Mat &pt2D, const Mat &pt3d, vector *Ps) { + assert(2 == pt2D.rows()); + assert(3 == pt3d.rows()); + assert(6 <= pt2D.cols()); + assert(pt2D.cols() == pt3d.cols()); + + //-- Translate 3D points in order to have X0 = (0,0,0,1). + Vec3 vecTranslation = - pt3d.col(0); + Mat4 translationMatrix = Mat4::Identity(); + translationMatrix.block<3,1>(0,3) = vecTranslation; + + Mat3X XPoints; + translate(pt3d, vecTranslation, &XPoints); + + std::vector vec_solution(11); + OSI_CLP_SolverWrapper wrapperLpSolve(vec_solution.size()); + Resection_L1_ConstraintBuilder cstBuilder(pt2D, XPoints); + if( + (BisectionLP( + wrapperLpSolve, + cstBuilder, + &vec_solution, + 1.0, + 0.0)) + ) + { + // Move computed value to dataset for residual estimation. + Mat34 P; + P << vec_solution[0], vec_solution[1], vec_solution[2], vec_solution[3], + vec_solution[4], vec_solution[5], vec_solution[6], vec_solution[7], + vec_solution[8], vec_solution[9], vec_solution[10], 1.0; + P = P * translationMatrix; + Ps->push_back(P); + } +} + +} // namespace kernel +} // namespace lInfinitycomputervision +} // namespace openMVG diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp b/src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp new file mode 100644 index 0000000000..98da264f91 --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp @@ -0,0 +1,50 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_RESECTION_L1_KERNEL_H_ +#define OPENMVG_RESECTION_L1_KERNEL_H_ + +#include +#include "openMVG/multiview/projection.hpp" +#include "openMVG/multiview/two_view_kernel.hpp" +#include "openMVG/numeric/numeric.h" + +namespace openMVG { +namespace lInfinitycomputervision { +namespace kernel { + +using namespace std; + +/** + * Six-point resection + * P Matrix estimation (Pose estimation) + * Rely on L1 Resection algorithm. + * Work from 6 to N points. + */ +struct l1SixPointResectionSolver { + enum { MINIMUM_SAMPLES = 6 }; + enum { MAX_MODELS = 1 }; + // Solve the problem of camera pose. + // First 3d point will be translated in order to have X0 = (0,0,0,1) + static void Solve(const Mat &pt2D, const Mat &pt3D, vector *P); + + // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) + static double Error(const Mat34 & P, const Vec2 & pt2D, const Vec3 & pt3D) + { + Vec2 x = Project(P, pt3D); + return (x-pt2D).norm(); + } +}; + +//-- Usable solver for the l1 6pt Resection Estimation +typedef two_view::kernel::Kernel l1PoseResectionKernel; + +} // namespace kernel +} // namespace lInfinitycomputervision +} // namespace openMVG + +#endif // OPENMVG_RESECTION_L1_KERNEL_H_ diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri.hpp b/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri.hpp new file mode 100644 index 0000000000..8c36830824 --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri.hpp @@ -0,0 +1,191 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_H_ +#define OPENMVG_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_H_ + +#include "openMVG/numeric/numeric.h" +#include "openMVG/linearProgramming/linearProgrammingInterface.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 openMVG { +namespace lInfinitycomputervision { + +using namespace linearProgramming; + +//-- Estimate the translation and the structure. +// and image points coordinates. +// - 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 +// + +/// Encode translation and structure linear program +void EncodeTiXi(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< std::pair > & 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 * (N3D + Ncam)); + + C.resize(5*Nobs, 1); + C.fill(0.0); + vec_sign.resize(5*Nobs + 3); + + const size_t transStart = 0; + const size_t pointStart = transStart + 3*Ncam; +# define TVAR(i, el) (0 + 3*(i) + (el)) +# define XVAR(j, el) (pointStart + 3*(j) + (el)) + + // By default set free variable: + vec_bounds = std::vector< std::pair >(3 * (N3D + Ncam)); + fill( vec_bounds.begin(), vec_bounds.end(), std::make_pair((double)-1e+30, (double)1e+30)); + // Fix the translation ambiguity. (set first cam at (0,0,0)) + vec_bounds[0] = vec_bounds[1] = vec_bounds[2] = make_pair(0,0); + + size_t rowPos = 0; + // Add the cheirality conditions (R_i*X_j + T_i)_3 + Z_ij >= 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; + C(rowPos) = 1.0; + vec_sign[rowPos] = LP_Constraints::LP_GREATER_OR_EQUAL; + ++rowPos; + + const Vec2 pt = M.block<2,1>(0,k); + const double u = pt(0); + const double v = pt(1); + + // 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; + C(rowPos) = 0.0; + vec_sign[rowPos] = LP_Constraints::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); + C(rowPos) = 0.0; + vec_sign[rowPos] = LP_Constraints::LP_LESS_OR_EQUAL; + ++rowPos; + + // y-residual => + // (R_i*X_j + T_i)_2 / (R_i*X_j + T_i)_3 - v >= -sigma + // (R_i*X_j + T_i)_2 - v * (R_i*X_j + T_i)_3 + sigma (R_i*X_j + T_i)_3 >= 0.0 + // R_i_3 * (sigma-v) + R_i_2 + t_i_2 + t_i_3 * (sigma-v) >= 0 + 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; + C(rowPos) = 0.0; + vec_sign[rowPos] = LP_Constraints::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); + C(rowPos) = 0.0; + vec_sign[rowPos] = LP_Constraints::LP_LESS_OR_EQUAL; + ++rowPos; + } +# undef TVAR +# undef XVAR +} + + +/// Kernel that set Linear constraints for the +/// - Translation Registration and Structure Problem. +/// Designed to be used with bisectionLP and LP_Solver interface. +/// +/// Implementation of problem of [1] -> 6.1 Cameras with known rotation +/// under a Linear Program form. (With SPARSE constraint matrix). + +struct Translation_Structure_L1_ConstraintBuilder +{ + Translation_Structure_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 LP_Constraints object. + bool Build(double gamma, LP_Constraints_Sparse & constraint) + { + EncodeTiXi(_M, _vec_Ri, + gamma, + constraint._constraintMat, + constraint._Cst_objective, + constraint._vec_sign, + constraint._vec_cost, + constraint._vec_bounds); + + //-- 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 Ncam = (size_t) _M.row(3).maxCoeff() + 1; + + constraint._nbParams = (Ncam + N3D) * 3; + + return true; + } + + std::vector _vec_Ri; // Rotation matrix + Mat _M; // M contains (X,Y,index3dPoint, indexCam)^T +}; + +} // namespace lInfinitycomputervision +} // namespace openMVG + +#endif // OPENMVG_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_H_ diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri_noise.hpp b/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri_noise.hpp new file mode 100644 index 0000000000..8390b4a9f1 --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri_noise.hpp @@ -0,0 +1,221 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_ +#define OPENMVG_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_ + +#include "openMVG/numeric/numeric.h" +#include "openMVG/linearProgramming/linearProgrammingInterface.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 openMVG { +namespace lInfinitycomputervision { + +using namespace linearProgramming; + +//-- Estimate the translation and the structure. +// and image points coordinates. +// - 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 +// variable for each x,y,Z residual. + +/// Encode translation and structure linear program with slack variables +/// in order to handle noisy measurements. +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< std::pair > & 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< std::pair >(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 + Z_ij >= 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] = LP_Constraints::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] = LP_Constraints::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] = LP_Constraints::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] = LP_Constraints::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] = LP_Constraints::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] = LP_Constraints::LP_EQUAL; ++rowPos; + A.coeffRef(rowPos, TVAR(0, 1)) = 1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LP_Constraints::LP_EQUAL; ++rowPos; + A.coeffRef(rowPos, TVAR(0, 2)) = 1.0; C(rowPos) = 0.0; vec_sign[rowPos] = LP_Constraints::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 LP_Constraints object. + bool Build(double gamma, LP_Constraints_Sparse & 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 lInfinitycomputervision +} // namespace openMVG + +#endif // OPENMVG_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_ diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_outlier_test.cpp b/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_outlier_test.cpp new file mode 100644 index 0000000000..80e05d19fb --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_outlier_test.cpp @@ -0,0 +1,258 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "openMVG/multiview/test_data_sets.hpp" +#include "openMVG/numeric/numeric.h" +#include "testing/testing.h" + +#include "openMVG/multiview/projection.hpp" + +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" + +#include "openMVG/linearProgramming/bisectionLP.hpp" +#include "openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri_noise.hpp" + +#include +#include + +using namespace openMVG; + +using namespace linearProgramming; +using namespace lInfinitycomputervision; + +TEST(Translation_Structure_L_Infinity_Noisy, Outlier_OSICLP_SOLVER) { + + const int nViews = 5; + const int nbPoints = 5; + const double focalValue = 1000; + const double cx = 500, cy = 500; + + const NViewDataSet d = + //NRealisticCamerasRing(nViews, nbPoints, + NRealisticCamerasCardioid(nViews, nbPoints, + nViewDatasetConfigurator(focalValue,focalValue,cx,cy,5,0)); + + d.ExportToPLY("test_Before_Infinity.ply"); + //-- Test triangulation of all the point + NViewDataSet d2 = d; + + //-- Set to 0 the future computed data to be sure of computation results : + d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation + fill(d2._t.begin(), d2._t.end(), Vec3(0.0,0.0,0.0)); + + { + // Prepare Rotation matrix (premultiplied by K) + // Apply the K matrix to the rotation + Mat3 K; + K << focalValue, 0, cx, + 0, focalValue, cy, + 0, 0, 1; + std::vector vec_KRotation(nViews); + for (size_t i = 0; i < nViews; ++i) { + vec_KRotation[i] = K * d._R[i]; + } + + //set two point as outlier + d2._x[0].col(0)(0) +=10; //Camera 0, measurement 0, noise on X coord + d2._x[3].col(3)(1) -=8; //Camera 3, measurement 3, noise on Y coord + + //Create the mega matrix + Mat megaMat(4, d._n*d._x[0].cols()); + { + int cpt = 0; + for (int i=0; i vec_solution((nViews + nbPoints + megaMat.cols())*3); + OSI_CLP_SolverWrapper wrapperOSICLPSolver(vec_solution.size()); + TiXi_withNoise_L1_ConstraintBuilder cstBuilder( vec_KRotation, megaMat); + std::cout << std::endl << "Bisection returns : " << std::endl; + std::cout<< BisectionLP( + wrapperOSICLPSolver, + cstBuilder, + &vec_solution, + admissibleResidual, + 0.0, 1e-8); + + std::cout << "Found solution:\n"; + std::copy(vec_solution.begin(), vec_solution.end(), std::ostream_iterator(std::cout, " ")); + + //-- First the ti and after the Xi : + + //-- Fill the ti + for (int i=0; i < nViews; ++i) { + d2._t[i] = K.inverse() * Vec3(vec_solution[3*i], vec_solution[3*i+1], vec_solution[3*i+2]); + // Change Ci to -Ri*Ci + d2._C[i] = -d2._R[i].inverse() * d2._t[i]; + } + + for (int i=0; i < nbPoints; ++i) { + size_t index = 3*nViews; + d2._X.col(i) = Vec3(vec_solution[index+i*3], vec_solution[index+i*3+1], vec_solution[index+i*3+2]); + } + + // Compute residuals L2 from estimated parameter values : + std::cout << std::endl << "Residual : " << std::endl; + Vec2 xk; + double xsum = 0.0; + for (int i = 0; i < d2._n; ++i) { + std::cout << "\nCamera : " << i << " \t:"; + for(int k = 0; k < d._x[0].cols(); ++k) + { + xk = Project(d2.P(i), Vec3(d2._X.col(k))); + double residual = (xk - d2._x[i].col(k)).norm(); + std::cout << Vec2(( xk - d2._x[i].col(k)).array().pow(2)).array().sqrt().mean() <<"\t"; + //-- Check that were measurement are not noisy the residual is small + // check were the measurement are noisy, the residual is important + //if ((i != 0 && k != 0) || (i!=3 && k !=3)) + if ((i != 0 && k != 0) && (i!=3 && k !=3)) { + EXPECT_NEAR(0.0, residual, 1e-6); + xsum += residual; + } + } + std::cout << std::endl; + } + double dResidual = xsum / (d2._n*d._x[0].cols()); + std::cout << std::endl << "Residual mean in not noisy measurement: " << dResidual << std::endl; + // Check that 2D re-projection and 3D point are near to GT. + EXPECT_NEAR(0.0, dResidual, 1e-1); + } + + d2.ExportToPLY("test_After_Infinity.ply"); +} + +#ifdef OPENMVG_HAVE_MOSEK +TEST(Translation_Structure_L_Infinity_Noisy, Outlier_MOSEK) { + + const int nViews = 5; + const int nbPoints = 5; + const double focalValue = 1000; + const double cx = 500, cy = 500; + + const NViewDataSet d = + //NRealisticCamerasRing(nViews, nbPoints, + NRealisticCamerasCardioid(nViews, nbPoints, + nViewDatasetConfigurator(focalValue,focalValue,cx,cy,5,0)); + + d.ExportToPLY("test_Before_Infinity.ply"); + //-- Test triangulation of all the point + NViewDataSet d2 = d; + + //-- Set to 0 the future computed data to be sure of computation results : + d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation + fill(d2._t.begin(), d2._t.end(), Vec3(0.0,0.0,0.0)); + + { + // Prepare Rotation matrix (premultiplied by K) + // Apply the K matrix to the rotation + Mat3 K; + K << focalValue, 0, cx, + 0, focalValue, cy, + 0, 0, 1; + std::vector vec_KRotation(nViews); + for (size_t i = 0; i < nViews; ++i) { + vec_KRotation[i] = K * d._R[i]; + } + + //set two point as outlier + d2._x[0].col(0)(0) +=10; //Camera 0, measurement 0, noise on X coord + d2._x[3].col(3)(1) -=8; //Camera 3, measurement 3, noise on Y coord + + //Create the mega matrix + Mat megaMat(4, d._n*d._x[0].cols()); + { + int cpt = 0; + for (int i=0; i vec_solution((nViews + nbPoints + megaMat.cols())*3); + MOSEK_SolveWrapper wrapperMosek(vec_solution.size()); + TiXi_withNoise_L1_ConstraintBuilder cstBuilder( vec_KRotation, megaMat); + std::cout << std::endl << "Bisection returns : " << std::endl; + std::cout<< BisectionLP( + wrapperMosek, + cstBuilder, + &vec_solution, + admissibleResidual, + 0.0, 1e-8); + + std::cout << "Found solution:\n"; + std::copy(vec_solution.begin(), vec_solution.end(), std::ostream_iterator(std::cout, " ")); + + //-- First the ti and after the Xi : + + //-- Fill the ti + for (int i=0; i < nViews; ++i) { + d2._t[i] = K.inverse() * Vec3(vec_solution[3*i], vec_solution[3*i+1], vec_solution[3*i+2]); + // Change Ci to -Ri*Ci + d2._C[i] = -d2._R[i].inverse() * d2._t[i]; + } + + for (int i=0; i < nbPoints; ++i) { + size_t index = 3*nViews; + d2._X.col(i) = Vec3(vec_solution[index+i*3], vec_solution[index+i*3+1], vec_solution[index+i*3+2]); + } + + // Compute residuals L2 from estimated parameter values : + std::cout << std::endl << "Residual : " << std::endl; + Vec2 xk; + double xsum = 0.0; + for (int i = 0; i < d2._n; ++i) { + std::cout << "\nCamera : " << i << " \t:"; + for(int k = 0; k < d._x[0].cols(); ++k) + { + xk = Project(d2.P(i), Vec3(d2._X.col(k))); + double residual = (xk - d2._x[i].col(k)).norm(); + std::cout << Vec2(( xk - d2._x[i].col(k)).array().pow(2)).array().sqrt().mean() <<"\t"; + //-- Check that were measurement are not noisy the residual is small + // check were the measurement are noisy, the residual is important + //if ((i != 0 && k != 0) || (i!=3 && k !=3)) + if ((i != 0 && k != 0) && (i!=3 && k !=3)) { + EXPECT_NEAR(0.0, residual, 1e-6); + xsum += residual; + } + } + std::cout << std::endl; + } + double dResidual = xsum / (d2._n*d._x[0].cols()); + std::cout << std::endl << "Residual mean in not noisy measurement: " << dResidual << std::endl; + // Check that 2D re-projection and 3D point are near to GT. + EXPECT_NEAR(0.0, dResidual, 1e-1); + } + + d2.ExportToPLY("test_After_Infinity.ply"); +} +#endif // OPENMVG_HAVE_MOSEK + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_test.cpp b/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_test.cpp new file mode 100644 index 0000000000..df8b748866 --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_test.cpp @@ -0,0 +1,289 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "openMVG/multiview/test_data_sets.hpp" +#include "openMVG/numeric/numeric.h" +#include "CppUnitLite/TestHarness.h" +#include "testing/testing.h" + +#include "openMVG/multiview/projection.hpp" + +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" + +#include "openMVG/linearProgramming/bisectionLP.hpp" +#include "openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri.hpp" + +#include +#include + +using namespace openMVG; + +using namespace linearProgramming; +using namespace lInfinitycomputervision; + +TEST(Translation_Structure_L_Infinity, OSICLP_SOLVER) { + + const size_t nViews = 3; + const size_t nbPoints = 6; + const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, + nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + + d.ExportToPLY("test_Before_Infinity.ply"); + //-- Test triangulation of all the point + NViewDataSet d2 = d; + + //-- Set to 0 the future computed data to be sure of computation results : + d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation + fill(d2._t.begin(), d2._t.end(), Vec3(0.0,0.0,0.0)); + + //Create the mega matrix + Mat megaMat(4, d._n*d._x[0].cols()); + { + size_t cpt = 0; + for (size_t i=0; i vec_solution((nViews + nbPoints)*3); + + OSI_CLP_SolverWrapper wrapperOSICLPSolver(vec_solution.size()); + Translation_Structure_L1_ConstraintBuilder cstBuilder( d._R, megaMat); + EXPECT_TRUE( + (BisectionLP( + wrapperOSICLPSolver, + cstBuilder, + &vec_solution, + 1.0, + 0.0)) + ); + + // Move computed value to dataset for residual estimation. + { + //-- Fill the ti + for (size_t i=0; i < nViews; ++i) + { + size_t index = i*3; + d2._t[i] = Vec3(vec_solution[index], vec_solution[index+1], vec_solution[index+2]); + // Change Ci to -Ri*Ci + d2._C[i] = -d2._R[i] * d2._t[i]; + } + + //-- Now the Xi : + for (size_t i=0; i < nbPoints; ++i) { + size_t index = 3*nViews; + d2._X.col(i) = Vec3(vec_solution[index+i*3], vec_solution[index+i*3+1], vec_solution[index+i*3+2]); + } + } + + // Compute residuals L2 from estimated parameter values : + Vec2 xk, xsum(0.0,0.0); + for (size_t i = 0; i < d2._n; ++i) { + for(size_t k = 0; k < d._x[0].cols(); ++k) + { + xk = Project(d2.P(i), Vec3(d2._X.col(k))); + xsum += Vec2(( xk - d2._x[i].col(k)).array().pow(2)); + } + } + double dResidual2D = (xsum.array().sqrt().sum()); + + // Check that 2D re-projection and 3D point are near to GT. + EXPECT_NEAR(0.0, dResidual2D, 1e-4); + } + + d2.ExportToPLY("test_After_Infinity.ply"); +} + +TEST(Translation_Structure_L_Infinity, OSICLP_SOLVER_K) { + + const size_t nViews = 3; + const size_t nbPoints = 6; + const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, + nViewDatasetConfigurator(1000,1000,500,500,5,0)); // Suppose a camera with Unit matrix as K + + d.ExportToPLY("test_Before_Infinity.ply"); + //-- Test triangulation of all the point + NViewDataSet d2 = d; + + //-- Set to 0 the future computed data to be sure of computation results : + d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation + fill(d2._t.begin(), d2._t.end(), Vec3(0.0,0.0,0.0)); + + //Create the mega matrix + Mat megaMat(4, d._n*d._x[0].cols()); + { + size_t cpt = 0; + for (size_t i=0; i vec_solution((nViews + nbPoints)*3); + + std::vector vec_KR(d._R); + for(int i=0;i < nViews; ++i) + vec_KR[i] = d._K[0] * d._R[i]; + + OSI_CLP_SolverWrapper wrapperOSICLPSolver(vec_solution.size()); + Translation_Structure_L1_ConstraintBuilder cstBuilder( vec_KR, megaMat); + EXPECT_TRUE( + (BisectionLP( + wrapperOSICLPSolver, + cstBuilder, + &vec_solution, + 1.0, + 0.0)) + ); + + // Move computed value to dataset for residual estimation. + { + //-- Fill the ti + for (size_t i=0; i < nViews; ++i) + { + size_t index = i*3; + d2._t[i] = d._K[0].inverse() * Vec3(vec_solution[index], vec_solution[index+1], vec_solution[index+2]); + // Change Ci to -Ri*Ci + d2._C[i] = -d2._R[i] * d2._t[i]; + } + + //-- Now the Xi : + for (size_t i=0; i < nbPoints; ++i) { + size_t index = 3*nViews; + d2._X.col(i) = Vec3(vec_solution[index+i*3], vec_solution[index+i*3+1], vec_solution[index+i*3+2]); + } + } + + // Compute residuals L2 from estimated parameter values : + Vec2 xk, xsum(0.0,0.0); + for (size_t i = 0; i < d2._n; ++i) { + for(size_t k = 0; k < d._x[0].cols(); ++k) + { + xk = Project(d2.P(i), Vec3(d2._X.col(k))); + xsum += Vec2(( xk - d2._x[i].col(k)).array().pow(2)); + } + } + double dResidual2D = (xsum.array().sqrt().sum()); + + // Check that 2D re-projection and 3D point are near to GT. + EXPECT_NEAR(0.0, dResidual2D, 1e-4); + } + + d2.ExportToPLY("test_After_Infinity.ply"); +} + +#ifdef OPENMVG_HAVE_MOSEK +TEST(Translation_Structure_L_Infinity, MOSEK) { + + const size_t nViews = 3; + const size_t nbPoints = 6; + const NViewDataSet d = NRealisticCamerasRing(nViews, nbPoints, + nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + + d.ExportToPLY("test_Before_Infinity.ply"); + //-- Test triangulation of all the point + NViewDataSet d2 = d; + + //-- Set to 0 the future computed data to be sure of computation results : + d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation + fill(d2._t.begin(), d2._t.end(), Vec3(0.0,0.0,0.0)); + + //Create the mega matrix + Mat megaMat(4, d._n*d._x[0].cols()); + { + size_t cpt = 0; + for (size_t i=0; i vec_solution((nViews + nbPoints)*3); + + MOSEK_SolveWrapper wrapperMosek(vec_solution.size()); + Translation_Structure_L1_ConstraintBuilder cstBuilder( d._R, megaMat); + EXPECT_TRUE( + (BisectionLP( + wrapperMosek, + cstBuilder, + &vec_solution, + 1.0, + 0.0)) + ); + + // Move computed value to dataset for residual estimation. + { + //-- Fill the ti + for (size_t i=0; i < nViews; ++i) + { + size_t index = i*3; + d2._t[i] = Vec3(vec_solution[index], vec_solution[index+1], vec_solution[index+2]); + // Change Ci to -Ri*Ci + d2._C[i] = -d2._R[i] * d2._t[i]; + } + + //-- Now the Xi : + for (size_t i=0; i < nbPoints; ++i) { + size_t index = 3*nViews; + d2._X.col(i) = Vec3(vec_solution[index+i*3], vec_solution[index+i*3+1], vec_solution[index+i*3+2]); + } + } + + // Compute residuals L2 from estimated parameter values : + Vec2 xk, xsum(0.0,0.0); + for (size_t i = 0; i < d2._n; ++i) { + for(size_t k = 0; k < d._x[0].cols(); ++k) + { + xk = Project(d2.P(i), Vec3(d2._X.col(k))); + xsum += Vec2(( xk - d2._x[i].col(k)).array().pow(2)); + } + } + double dResidual2D = (xsum.array().sqrt().sum()); + + // Check that 2D re-projection and 3D point are near to GT. + EXPECT_NEAR(0.0, dResidual2D, 1e-4); + } + + d2.ExportToPLY("test_After_Infinity.ply"); +} +#endif // OPENMVG_HAVE_MOSEK + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/triangulation.hpp b/src/openMVG/linearProgramming/lInfinitycomputervision/triangulation.hpp new file mode 100644 index 0000000000..4e4a739042 --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/triangulation.hpp @@ -0,0 +1,126 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_LINFINITY_COMPUTER_VISION_TRIANGULATION_H_ +#define OPENMVG_LINFINITY_COMPUTER_VISION_TRIANGULATION_H_ + +#include "openMVG/numeric/numeric.h" +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#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 openMVG { +namespace lInfinitycomputervision { + +using namespace linearProgramming; + +//-- Triangulation +// - Estimation of X from Pi and xij +// [1] -> 5.1 The triangulation problem +// +// - 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 +// + +void EncodeTriangulation(const std::vector & Pi, + const Mat2X & x_ij, + double gamma, // Start upper bound + Mat & A, Vec & C) +{ + // Build A, C matrix. + + const size_t nbCamera = Pi.size(); + A.resize(5*nbCamera,3); + C.resize(5*nbCamera,1); + + int cpt = 0; + for (int i = 0; i< nbCamera; ++i) + { + const Mat3 R = Pi[i].block<3,3>(0,0); + const Vec3 t = Pi[i].block<3,1>(0,3); + const Mat2X pt = x_ij.col(i); + + // A (Rotational part): + A.block<1,3>(cpt,0) = R.row(0) - pt(0) * R.row(2) - gamma * R.row(2); + A.block<1,3>(cpt+1,0) = R.row(1) - pt(1) * R.row(2) - gamma * R.row(2); + A.block<1,3>(cpt+2,0) = - R.row(2); + A.block<1,3>(cpt+3,0) = - R.row(0) + pt(0) * R.row(2) - gamma * R.row(2); + A.block<1,3>(cpt+4,0) = - R.row(1) + pt(1) * R.row(2) - gamma * R.row(2); + + // C (translation part): + C(cpt) = - t(0) + pt(0) * t(2) + gamma * t(2); + C(cpt+1) = - t(1) + pt(1) * t(2) + gamma * t(2); + C(cpt+2) = t(2); + C(cpt+3) = t(0) - pt(0) * t(2) + gamma * t(2); + C(cpt+4) = t(1) - pt(1) * t(2) + gamma * t(2); + + //- Next entry + cpt += 5; + } +} + +/// Kernel that set Linear constraints for the Triangulation Problem. +/// Designed to be used with bisectionLP and LP_Solver interface. +/// +/// Triangulation : +/// - Estimation of Xi from Pj and xij +/// Implementation of problem of [1] -> 5.1 The triangulation problem +/// under a Linear Program form. +struct Triangulation_L1_ConstraintBuilder +{ + Triangulation_L1_ConstraintBuilder( + const std::vector & vec_Pi, + const Mat2X & x_ij) + { + _vec_Pi = vec_Pi; + _x_ij = x_ij; + } + + /// Setup constraints of the triangulation problem as a Linear program + bool Build(double gamma, LP_Constraints & constraint) + { + EncodeTriangulation(_vec_Pi, _x_ij, + gamma, + constraint._constraintMat, + constraint._Cst_objective); + //-- Setup additional information about the Linear Program constraint + // We look for 3 variables [X,Y,Z] with no bounds. + // Constraint sign are all less or equal (<=) + constraint._nbParams = 3; + constraint._vec_bounds = std::vector< std::pair >(1); + fill(constraint._vec_bounds.begin(),constraint._vec_bounds.end(), + std::make_pair((double)-1e+30, (double)1e+30)); + // Setup constraint sign + constraint._vec_sign.resize(constraint._constraintMat.rows()); + fill(constraint._vec_sign.begin(), constraint._vec_sign.end(), + LP_Constraints::LP_LESS_OR_EQUAL); + + return true; + } + + //-- Data required for triangulation : + std::vector _vec_Pi; // Projection matrix + Mat2X _x_ij; // 2d projection : xij = Pj*Xi +}; + + +} // namespace lInfinitycomputervision +} // namespace openMVG + +#endif // OPENMVG_LINFINITY_COMPUTER_VISION_TRIANGULATION_H_ diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/triangulation_L_Infty_test.cpp b/src/openMVG/linearProgramming/lInfinitycomputervision/triangulation_L_Infty_test.cpp new file mode 100644 index 0000000000..472a251a76 --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/triangulation_L_Infty_test.cpp @@ -0,0 +1,149 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include +#include + +#include "openMVG/multiview/test_data_sets.hpp" +#include "openMVG/numeric/numeric.h" +#include "testing/testing.h" + +#include "openMVG/multiview/projection.hpp" + +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" + +#include "openMVG/linearProgramming/bisectionLP.hpp" +#include "openMVG/linearProgramming/lInfinitycomputervision/triangulation.hpp" + +using namespace openMVG; +using namespace linearProgramming; +using namespace lInfinitycomputervision; + +TEST(lInfinitycomputervision, Triangulation_OSICLPSOLVER) { + + NViewDataSet d = NRealisticCamerasRing(6, 10, + nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + + std::vector vec_Pi; + + d.ExportToPLY("test_Before_Infinity_Triangulation_OSICLP.ply"); + //-- Test triangulation of all the point + NViewDataSet d2 = d; + d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation + + for (int i = 0; i < d._n; ++i) + vec_Pi.push_back(d.P(i)); + + for (int k = 0; k < d._x[0].cols(); ++k) + { + Mat2X x_ij; + x_ij.resize(2,d._n); + for (int i = 0; i < d._n; ++i) + x_ij.col(i) = d._x[i].col(k); + + std::vector vec_solution(3); + + OSI_CLP_SolverWrapper wrapperOSICLPSolver(3); + Triangulation_L1_ConstraintBuilder cstBuilder(vec_Pi, x_ij); + // Use bisection in order to find the global optimum and so find the + // best triangulated point under the L_infinity norm + EXPECT_TRUE( + (BisectionLP( + wrapperOSICLPSolver, + cstBuilder, + &vec_solution, + 1.0, + 0.0)) + ); + + Vec3 XSolution(vec_solution[0], vec_solution[1], vec_solution[2]); + d2._X.col(k) = XSolution.transpose(); + + // Compute residuals L2 from estimated parameter values : + const Vec3 & X = XSolution; + Vec2 x1, xsum(0.0,0.0); + for (int i = 0; i < d2._n; ++i) { + x1 = Project(d2.P(i), X); + xsum += Vec2((x1-x_ij.col(i)).array().pow(2)); + } + double dResidual2D = (xsum.array().sqrt().sum()); + + // Residual LInfinity between GT 3D point and found one + double dResidual3D = DistanceLInfinity(XSolution, Vec3(d._X.col(k))); + + // Check that 2D re-projection and 3D point are near to GT. + EXPECT_NEAR(0.0, dResidual2D, 1e-5); + EXPECT_NEAR(0.0, dResidual3D, 1e-5); + } + d2.ExportToPLY("test_After_Infinity_Triangulation_OSICLP.ply"); +} + +#ifdef OPENMVG_HAVE_MOSEK +TEST(computervision, Triangulation_MOSEK) { + + NViewDataSet d = NRealisticCamerasRing(6, 10, + nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K + + std::vector vec_Pi; + + d.ExportToPLY("test_Before_Infinity_Triangulation_MOSEK.ply"); + //-- Test triangulation of all the point + NViewDataSet d2 = d; + d2._X.fill(0); //Set _Xi of dataset 2 to 0 to be sure of new data computation + + for (int i = 0; i < d._n; ++i) + vec_Pi.push_back(d.P(i)); + + for (int k = 0; k < d._x[0].cols(); ++k) + { + Mat2X x_ij; + x_ij.resize(2,d._n); + for (int i = 0; i < d._n; ++i) + x_ij.col(i) = d._x[i].col(k); + + std::vector vec_solution(3); + + MOSEK_SolveWrapper wrapperLpSolve(3); + Triangulation_L1_ConstraintBuilder cstBuilder(vec_Pi, x_ij); + // Use bisection in order to find the global optimum and so find the + // best triangulated point under the L_infinity norm + EXPECT_TRUE( + (BisectionLP( + wrapperLpSolve, + cstBuilder, + &vec_solution, + 1.0, + 0.0)) + ); + + Vec3 XSolution(vec_solution[0], vec_solution[1], vec_solution[2]); + d2._X.col(k) = XSolution.transpose(); + + // Compute residuals L2 from estimated parameter values : + const Vec3 & X = XSolution; + Vec2 x1, xsum(0.0,0.0); + for (int i = 0; i < d2._n; ++i) { + x1 = Project(d2.P(i), X); + xsum += Vec2((x1-x_ij.col(i)).array().pow(2)); + } + double dResidual2D = (xsum.array().sqrt().sum()); + + // Residual LInfinity between GT 3D point and found one + double dResidual3D = DistanceLInfinity(XSolution, Vec3(d._X.col(k))); + + // Check that 2D re-projection and 3D point are near to GT. + EXPECT_NEAR(0.0, dResidual2D, 1e-5); + EXPECT_NEAR(0.0, dResidual3D, 1e-5); + } + d2.ExportToPLY("test_After_Infinity_Triangulation_MOSEK.ply"); +} +#endif // OPENMVG_HAVE_MOSEK + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From d946777ddd5a3489a9ae58621bcc0ca070f3de60 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 30 Dec 2013 10:55:53 +0100 Subject: [PATCH 05/68] Add a graph module to openMVG. Add easy computation of connected component of graphs. #50 --- src/openMVG/CMakeLists.txt | 2 +- src/openMVG/graph/CMakeLists.txt | 3 + src/openMVG/graph/connectedComponent.hpp | 42 +++++++ src/openMVG/graph/connectedComponent_test.cpp | 114 ++++++++++++++++++ 4 files changed, 160 insertions(+), 1 deletion(-) create mode 100644 src/openMVG/graph/CMakeLists.txt create mode 100644 src/openMVG/graph/connectedComponent.hpp create mode 100644 src/openMVG/graph/connectedComponent_test.cpp diff --git a/src/openMVG/CMakeLists.txt b/src/openMVG/CMakeLists.txt index ebda00e439..8e04220cdc 100644 --- a/src/openMVG/CMakeLists.txt +++ b/src/openMVG/CMakeLists.txt @@ -4,7 +4,7 @@ ADD_SUBDIRECTORY(matching) ADD_SUBDIRECTORY(multiview) ADD_SUBDIRECTORY(numeric) ADD_SUBDIRECTORY(tracks) - +ADD_SUBDIRECTORY(graph) ADD_SUBDIRECTORY(robust_estimation) ADD_SUBDIRECTORY(bundle_adjustment) diff --git a/src/openMVG/graph/CMakeLists.txt b/src/openMVG/graph/CMakeLists.txt new file mode 100644 index 0000000000..21142912e4 --- /dev/null +++ b/src/openMVG/graph/CMakeLists.txt @@ -0,0 +1,3 @@ + +UNIT_TEST(openMVG connectedComponent "lemon") + diff --git a/src/openMVG/graph/connectedComponent.hpp b/src/openMVG/graph/connectedComponent.hpp new file mode 100644 index 0000000000..aaf9de69d9 --- /dev/null +++ b/src/openMVG/graph/connectedComponent.hpp @@ -0,0 +1,42 @@ +// Copyright (c) 2012, 2013 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_GRAPH_CONNECTED_COMPONENT_H_ +#define OPENMVG_GRAPH_CONNECTED_COMPONENT_H_ + +#include +#include + +namespace openMVG +{ +namespace graph +{ + +/// Export node of each CC (Connected Component) in a map +template +std::map > exportGraphToMapSubgraphs( + const GraphT & g) +{ + typedef lemon::ListGraph::NodeMap IndexMap; + IndexMap connectedNodeMap(g); + int connectedComponentCount = lemon::connectedComponents(g, connectedNodeMap); + + std::map > map_subgraphs; + + // Create subgraphs' map + typedef lemon::ListGraph::NodeIt NodeIterator; + NodeIterator itNode(g); + for (IndexMap::MapIt it(connectedNodeMap); + it != lemon::INVALID; ++it, ++itNode) { + map_subgraphs[*it].insert(itNode); + } + return map_subgraphs; +} + +} // namespace graph +} // namespace openMVG + +#endif // OPENMVG_GRAPH_CONNECTED_COMPONENT_H_ diff --git a/src/openMVG/graph/connectedComponent_test.cpp b/src/openMVG/graph/connectedComponent_test.cpp new file mode 100644 index 0000000000..9afb22ebed --- /dev/null +++ b/src/openMVG/graph/connectedComponent_test.cpp @@ -0,0 +1,114 @@ +// Copyright (c) 2012, 2013 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#include +#include + +#include "CppUnitLite/TestHarness.h" +#include "testing/testing.h" + +#include "openMVG/graph/connectedComponent.hpp" +#include "lemon/list_graph.h" +using namespace lemon; + +TEST(connectedComponents, Empty) { + lemon::ListGraph graph; + + int connectedComponentCount = lemon::countConnectedComponents(graph); + EXPECT_EQ(0, connectedComponentCount); +} + +TEST(connectedComponents, OneCC) { + lemon::ListGraph graph; + lemon::ListGraph::Node a = graph.addNode(), b = graph.addNode(); + graph.addEdge(a,b); + int connectedComponentCount = lemon::countConnectedComponents(graph); + EXPECT_EQ(1, connectedComponentCount); +} + +TEST(connectedComponents, TwoCC) { + lemon::ListGraph graph; + + lemon::ListGraph::Node a = graph.addNode(), b = graph.addNode(); + graph.addEdge(a,b); + lemon::ListGraph::Node a2 = graph.addNode(), b2 = graph.addNode(); + graph.addEdge(a2,b2); + int connectedComponentCount = lemon::countConnectedComponents(graph); + EXPECT_EQ(2, connectedComponentCount); +} + + +TEST(connectedComponents, TwoCC_Parsing) { + lemon::ListGraph graph; + + lemon::ListGraph::Node a = graph.addNode(), b = graph.addNode(); + graph.addEdge(a,b); + lemon::ListGraph::Node a2 = graph.addNode(), b2 = graph.addNode(); + graph.addEdge(a2,b2); + + typedef ListGraph::NodeMap IndexMap; + IndexMap connectedNodeMap(graph); + int connectedComponentCount = lemon::connectedComponents(graph, connectedNodeMap); + EXPECT_EQ(2, connectedComponentCount); + for (IndexMap::MapIt it(connectedNodeMap); it != INVALID; ++it) + { + std::cout << *it << "\t" << graph.id(it) << "\n"; + } +} + + +/// Test to get back node id of each CC +// a +// +// b-c +// +// d-g +// | | +// e-f +// +// h-i-j-k +// |/ +// l +TEST(exportGraphToMapSubgraphs, CC_Subgraph) { + lemon::ListGraph graph; + + // single + lemon::ListGraph::Node a = graph.addNode(); + + // two + lemon::ListGraph::Node b = graph.addNode(), c = graph.addNode(); + graph.addEdge(b,c); + + // four + lemon::ListGraph::Node d = graph.addNode(), e = graph.addNode(), + f = graph.addNode(), g = graph.addNode(); + graph.addEdge(d,e); + graph.addEdge(e,f); + graph.addEdge(f,g); + graph.addEdge(g,d); + + // five + lemon::ListGraph::Node h = graph.addNode(), i = graph.addNode(), + j = graph.addNode(), k = graph.addNode(),l = graph.addNode(); + graph.addEdge(h,i); + graph.addEdge(i,j); + graph.addEdge(j,k); + graph.addEdge(i,l); + graph.addEdge(j,l); + + const std::map > map_subgraphs = + openMVG::graph::exportGraphToMapSubgraphs(graph); + + EXPECT_EQ(4, map_subgraphs.size()); + EXPECT_EQ(5, map_subgraphs.at(0).size()); + EXPECT_EQ(4, map_subgraphs.at(1).size()); + EXPECT_EQ(2, map_subgraphs.at(2).size()); + EXPECT_EQ(1, map_subgraphs.at(3).size()); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From 24ccd08feaf598ce1ad780efe65da0f89a450ae0 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 30 Dec 2013 11:50:00 +0100 Subject: [PATCH 06/68] Add an implementation of K-VLD paper. #51 --- src/openMVG/matching/CMakeLists.txt | 2 + src/openMVG/matching/kvld/CMakeLists.txt | 5 + src/openMVG/matching/kvld/algorithm.cpp | 55 ++ src/openMVG/matching/kvld/algorithm.h | 201 +++++++ src/openMVG/matching/kvld/kvld.cpp | 513 ++++++++++++++++++ src/openMVG/matching/kvld/kvld.h | 193 +++++++ src/openMVG_Samples/CMakeLists.txt | 2 + .../kvld_filter/CMakeLists.txt | 15 + .../kvld_filter/kvld_filter.cpp | 276 ++++++++++ 9 files changed, 1262 insertions(+) create mode 100644 src/openMVG/matching/kvld/CMakeLists.txt create mode 100644 src/openMVG/matching/kvld/algorithm.cpp create mode 100644 src/openMVG/matching/kvld/algorithm.h create mode 100644 src/openMVG/matching/kvld/kvld.cpp create mode 100644 src/openMVG/matching/kvld/kvld.h create mode 100644 src/openMVG_Samples/kvld_filter/CMakeLists.txt create mode 100644 src/openMVG_Samples/kvld_filter/kvld_filter.cpp diff --git a/src/openMVG/matching/CMakeLists.txt b/src/openMVG/matching/CMakeLists.txt index 16d6d328b1..d9caa23b43 100644 --- a/src/openMVG/matching/CMakeLists.txt +++ b/src/openMVG/matching/CMakeLists.txt @@ -22,3 +22,5 @@ set_source_files_properties(${matching_files_cpp} PROPERTIES LANGUAGE CXX) UNIT_TEST(openMVG matching "flann_cpp_s") UNIT_TEST(openMVG matching_filters "") UNIT_TEST(openMVG indMatch "") + +ADD_SUBDIRECTORY(kvld) diff --git a/src/openMVG/matching/kvld/CMakeLists.txt b/src/openMVG/matching/kvld/CMakeLists.txt new file mode 100644 index 0000000000..58a56ca692 --- /dev/null +++ b/src/openMVG/matching/kvld/CMakeLists.txt @@ -0,0 +1,5 @@ + +ADD_LIBRARY(openMVG_kvld kvld.cpp kvld.h algorithm.cpp algorithm.h) +TARGET_LINK_LIBRARIES(openMVG_kvld openMVG_image) + + diff --git a/src/openMVG/matching/kvld/algorithm.cpp b/src/openMVG/matching/kvld/algorithm.cpp new file mode 100644 index 0000000000..3467443924 --- /dev/null +++ b/src/openMVG/matching/kvld/algorithm.cpp @@ -0,0 +1,55 @@ +/** @basic structures implementation + ** @author Zhe Liu + **/ + +/* +Copyright (C) 2011-12 Zhe Liu and Pierre Moulon. +All rights reserved. + +This file is part of the KVLD library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#include "algorithm.h" + +IntegralImages::IntegralImages( const Image< float >& I ) +{ + map.resize( I.Width() + 1, I.Height() + 1 ); + map.fill( 0 ); + for( int y = 0; y < I.Height(); y++ ) + { + for( int x = 0; x < I.Width(); x++ ) + { + map( y + 1, x + 1 ) = double( I( y, x ) ) + map( y, x + 1 ) + map( y + 1, x ) - map( y, x ); + } + } +} + +float getRange( const Image< float >& I, + int a, + const float p ) +{ + float range = sqrt( float( 3.f * I.Height() * I.Width() ) / ( p * a * PI_ ) ); + return range; +} + + +//=============================IO interface======================// + +std::ofstream& writeDetector( std::ofstream& out, const openMVG::SIOPointFeature& feature ) +{ + out << feature.x() << " " + << feature.y() << " " + << feature.scale() << " " + << feature.orientation() <> point.x() + >> point.y() + >> point.scale() + >> point.orientation(); + return in; +} diff --git a/src/openMVG/matching/kvld/algorithm.h b/src/openMVG/matching/kvld/algorithm.h new file mode 100644 index 0000000000..b5b5fa57be --- /dev/null +++ b/src/openMVG/matching/kvld/algorithm.h @@ -0,0 +1,201 @@ +#ifndef KVLD_ALGORITHM_H +#define KVLD_ALGORITHM_H + +/** @basic structures implementation + ** @author Zhe Liu + **/ + +/* +Copyright (C) 2011-12 Zhe Liu and Pierre Moulon. +All rights reserved. + +This file is part of the KVLD library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +typedef std::pair< size_t, size_t > Pair; +const float PI_ = 4.0 * atan( 1.0f ); + +//============================== simplified structure of a point=============================// +//if you set KVLD geometry verification to false, you only need to fill x and y in a point structure +struct PointS +{ + float x, y; // point position + float scale; // point scale + float angle; // point orientation + + PointS( float x = (0.f), float y = (0.f)): + x( x ), y( y ){} + PointS( const float& x, const float& y,const float& angle,const float& scale): + x( x ), y( y ), angle( angle ), scale( scale ){} +}; + +//===================================== integral image ====================================// +//It is used to efficiently construct the pyramid of scale images in KVLD +struct IntegralImages +{ + Image< double > map; + + IntegralImages( const Image< float >& I ); + + inline double operator()( double x1, double y1, double x2, double y2 )const + { + return get( x2, y2 ) - get( x1, y2 ) - get( x2, y1 ) + get( x1, y1 ); + } + inline double operator()( double x, double y, double size ) const + { + double window = 0.5 * size; + return ( get( x + window, y + window ) - get( x - window, y + window ) - get( x + window, y - window ) + get( x - window, y - window ) ) / ( 4 * window * window ); + } +private : + inline double get( double x, double y )const + { + int ix = int( x ); + int iy = int( y ); + double dx = x - ix; + double dy = y - iy; + if( dx == 0 && dy == 0 ) + return map( iy, ix ); + if( dx == 0 ) + return map( iy, ix ) * ( 1 - dy ) + map( iy + 1, ix ) * dy; + if( dy == 0 ) + return map( iy, ix ) * ( 1 - dx ) + map( iy, ix + 1 ) * dx; + + return map( iy, ix ) * ( 1 - dx ) * ( 1 - dy ) + + map( iy + 1, ix ) * dy * ( 1 - dx ) + + map( iy, ix + 1 ) * ( 1 - dy ) * dx + + map( iy + 1, ix + 1 ) * dx * dy; + } +}; + +//=============================IO interface ======================// + +std::ofstream& writeDetector( std::ofstream& out, const openMVG::SIOPointFeature& vect ); +std::ifstream& readDetector( std::ifstream& in, openMVG::SIOPointFeature& point ); +//======================================elemetuary operations================================// +template < typename T > +inline T point_distance( const T x1, const T y1, const T x2, const T y2 ) +{//distance of points + float a = x1 - x2; + float b = y1 - y2; + return sqrt( a * a + b * b ); +} + +template < typename T > +inline float point_distance( const T& P1, const T& P2 ) +{//distance of points + return point_distance< float >( P1.x(), P1.y(), P2.x(), P2.y() ); +} + +inline bool inside( int w, int h, int x,int y, double radius ) +{ + return( x - radius >= 0 && y - radius >= 0 && x + radius < w && y + radius < h ); +} + +inline bool anglefrom( const float& x, const float& y, float& angle ) +{ + if( x != 0 ) + angle = atan( y / x ); + else if( y > 0 ) + angle = PI_ / 2; + else if( y < 0 ) + angle =- PI_ / 2; + else return false; + + if( x < 0 ) + angle += PI_; + while( angle < 0 ) + angle += 2 * PI_; + while( angle >= 2 * PI_ ) + angle -= 2 * PI_; + assert( angle >= 0 && angle < 2 * PI_ ); + return true; +} + +inline double angle_difference( const double angle1, const double angle2 ) +{ + double angle = angle1 - angle2; + while( angle < 0 ) angle += 2 * PI_; + while( angle >= 2 * PI_ ) angle -= 2 * PI_; + + assert(angle <= 2 * PI_ && angle >= 0 ); + return std::min( angle, 2 * PI_ - angle ); +} + +inline void max( double* list,double& weight, int size, int& index, int& second_index ) +{ + index = 0; + second_index = -1; + double best = list[ index ] - list[ index + size / 2 ]; + + for( int i = 0; i < size; i++ ) + { + double value; + if( i < size / 2) value = list[ i ] - list[ i + size / 2 ]; + else value = list[ i ] - list[ i - size / 2 ]; + + if( value > best ) + { + best = value; + second_index = index; + index = i; + } + } + weight = best; +} + +template< typename ARRAY > +inline void normalize_weight( ARRAY & weight ) +{ + double total = weight.array().sum(); + if( !total == 0 ) + for( int i = 0; i < weight.size(); i++ ) + weight[ i ] /= total; +} + +template< typename T > +inline float consistent( const T& a1, const T& a2, const T& b1, const T& b2 ) +{ + float ax = float( a1.x() - a2.x() ); + float ay = float( a1.y() - a2.y() ); + float bx = float( b1.x() - b2.x() ); + float by = float( b1.y() - b2.y() ); + + float angle1 = float( b1.orientation() - a1.orientation() ); + float angle2 = float( b2.orientation() - a2.orientation() ); + + float ax1 = cos( angle1 ) * ax - sin( angle1 ) * ay; + ax1 *= float( b1.scale() / a1.scale() ); + float ay1 = sin( angle1 ) * ax + cos( angle1 ) * ay; + ay1 *= float( b1.scale() / a1.scale() ); + float d1 = sqrt( ax1 * ax1 + ay1 * ay1 ); + float d1_error = sqrt( ( ax1 - bx ) * ( ax1 - bx ) + ( ay1 - by ) * ( ay1 - by ) ); + + float ax2 = float( cos( angle2 ) * ax - sin( angle2 ) * ay ); + ax2 *= float( b2.scale() / a2.scale() ); + + float ay2 = float( sin( angle2 ) * ax + cos( angle2 ) * ay ); + ay2 *= float( b2.scale() / a2.scale() ); + float d2 = sqrt( ax2 * ax2 + ay2 * ay2 ); + float d2_error = sqrt( ( ax2 - bx ) * ( ax2 - bx ) + ( ay2 - by ) * ( ay2 - by ) ); + float d = std::min( d1_error / std::min( d1, point_distance( b1, b2 ) ), d2_error / std::min( d2, point_distance( b1, b2 ) ) ); + return d; +} +float getRange( const Image< float >& I, int a, const float p ); + +#endif //KVLD_ALGORITHM_H diff --git a/src/openMVG/matching/kvld/kvld.cpp b/src/openMVG/matching/kvld/kvld.cpp new file mode 100644 index 0000000000..fd9567d89c --- /dev/null +++ b/src/openMVG/matching/kvld/kvld.cpp @@ -0,0 +1,513 @@ +/** @Main KVLD algorithm implementation + ** @Containing scale image pyramid, VLD structure and KVLD algorithm + ** @author Zhe Liu + **/ + +/* +Copyright (C) 2011-12 Zhe Liu and Pierre Moulon. +All rights reserved. + +This file is part of the KVLD library and is made available under +the terms of the BSD license ( see the COPYING file). +*/ + +#include "kvld.h" +#include "algorithm.h" +#include +#include +#include + +using namespace std; + +ImageScale::ImageScale( const Image< float >& I, double r ) +{ + IntegralImages inter( I ); + radius_size = r; + step = sqrt( 2.0 ); + int size = max( I.Width(),I.Height() ); + + int number= int( log( size / r ) / log( 2.0 ) ) + 1; + angles.resize( number ); + magnitudes.resize( number ); + ratios.resize( number ); + + GradAndNorm( I, angles[ 0 ], magnitudes[ 0 ] ); + ratios[ 0 ] = 1; + +#ifdef _OPENMP +#pragma omp parallel for +#endif + for( int k = 1; k < number; k++ ) + { + Image< float > I2; + double ratio = 1 * pow( step, k ); + I2.resize( int( I.Width() / ratio ), int( I.Height() / ratio ) ); + angles[ k ].resize( int( I.Width() / ratio ), int( I.Height() / ratio ) ); + magnitudes[ k ].resize( int( I.Width() / ratio ), int( I.Height() / ratio ) ); + + for( int i = 0; i < I2.Width(); i++ ) + { + for( int j = 0; j < I2.Height(); j++ ) + { + I2( j, i ) = inter( double( i + 0.5 ) * ratio, double( j + 0.5 ) * ratio, ratio ); + } + } + GradAndNorm( I2,angles[ k ], magnitudes[ k ] ); + ratios[ k ] = ratio; + } +} + +void ImageScale::GradAndNorm( const Image< float >& I, Image< float >& angle, Image< float >& m ) +{ + angle = Image< float >( I.Width(), I.Height() ); + m = Image< float >( I.Width(), I.Height() ); + angle.fill( 0 ); + m.fill( 0 ); +#ifdef _OPENMP +#pragma omp parallel for +#endif + for( int x = 1; x < I.Width() - 1; x++ ) + { + for( int y = 1; y < I.Height() - 1; y++ ) + { + float gx = I( y, x + 1 ) - I( y, x - 1 ); + float gy = I( y + 1, x ) - I( y - 1, x ); + + if( !anglefrom( gx, gy, angle( y, x ) ) ) + angle( y, x ) = -1; + m( y, x ) = sqrt( gx * gx + gy * gy ); + } + } +} + +int ImageScale::getIndex( const double r )const +{ + const double step = sqrt( 2.0 ); + + if( r <= radius_size ) return 0; + else + { + double range_low = radius_size; + int index = 0; + while( r > range_low * step ) + { + index++; + range_low *= step; + } + return index; + } +} + +template< typename T > +VLD::VLD( const ImageScale& series, T const& P1, T const& P2 ) : contrast( 0.0 ) +{ + //============== initializing============// + principleAngle.fill( 0 ); + descriptor.fill( 0 ); + weight.fill( 0 ); + + begin_point[ 0 ] = P1.x(); + begin_point[ 1 ] = P1.y(); + end_point[ 0 ] = P2.x(); + end_point[ 1 ] = P2.y(); + + float dy = float( end_point[ 1 ] - begin_point[ 1 ] ); + float dx = float( end_point[ 0 ] - begin_point[ 0 ] ); + distance = sqrt( dy * dy + dx * dx ); + + if( distance == 0 ) + cerr<<"Two SIFT points have the same coordinate"< & ang = series.angles[ image_index ]; + const Image< float > & m = series.magnitudes[ image_index ]; + double ratio = series.ratios[ image_index ]; + +// cout<= 0 ) + angle = ang( y, x ) - mainAngle;//relative angle + else angle = 0.0; + + //cout<= 2 * PI_) + angle -= 2 * PI_; + + //===============principle angle==============================// + int index = int( angle * binNum / ( 2 * PI_ ) + 0.5 ); + + double Gweight = exp( -d * d / 4.5 / sigma2 ) * ( m( y, x ) ); + // cout<<"in number "<= 0 && index2 <= subdirection ); + + if( index2 < subdirection ) + descriptor[ subdirection * i + index2 ] += Gweight; + else descriptor[ subdirection * i ] += Gweight;// possible since the 0.5 + } + } + } + //=====================find the biggest angle of ith SIFT==================// + int index; + int second_index; + max( statistic, weight[ i ], binNum, index, second_index ); + principleAngle[ i ] = index; + } + + normalize_weight( descriptor ); + + contrast = weight.array().sum(); + contrast /= distance / ratio; + normalize_weight( weight ); +} + +float KVLD( const Image< float >& I1, + const Image< float >& I2, + vector< openMVG::SIOPointFeature >& F1, + vector< openMVG::SIOPointFeature >& F2, + const vector< Pair >& matches, + vector< Pair >& matchesFiltered, + vector< double >& score, + openMVG::Mat& E, + vector< bool >& valide, + KvldParameters& kvldParameters ) +{ + matchesFiltered.clear(); + score.clear(); + ImageScale Chaine1( I1 ); + ImageScale Chaine2( I2 ); + + cout << "Image scale-space complete..." << endl; + + float range1 = getRange( I1, min( F1.size(), matches.size() ), kvldParameters.inlierRate ); + float range2 = getRange( I2, min( F2.size(), matches.size() ), kvldParameters.inlierRate ); + + size_t size = matches.size(); + + //================distance map construction, foruse of selecting neighbors===============// + cout << "computing distance maps" << endl; + + bool bPrecomputedDist = false; + + openMVG::Matf dist1, dist2; + if( bPrecomputedDist ) + { + dist1 = openMVG::Matf::Zero( F1.size(), F1.size() ); + dist2 = openMVG::Matf::Zero( F2.size(), F2.size() ); + + for( int a1 = 0; a1 < F1.size(); ++a1 ) + for( int a2 = a1; a2 < F1.size(); ++a2 ) + dist1( a1, a2 ) = dist1( a2, a1 ) = point_distance( F1[ a1 ], F1[ a2 ] ); + + for( int b1 = 0; b1 < F2.size(); ++b1 ) + for( int b2 = b1; b2 < F2.size(); ++b2 ) + dist2( b1, b2 ) = dist2( b2, b1 ) = point_distance( F2[ b1 ], F2[ b2 ] ); + } + + fill( valide.begin(), valide.end(), true ); + vector< double > scoretable( size, 0.0 ); + vector< size_t > result( size, 0 ); + + +//============main iteration formatch verification==========// +// cout<<"main iteration"; + bool change = true; + bool initial = true; + + while( change ) + { + change = false; + + fill( scoretable.begin(), scoretable.end(), 0.0 ); + fill( result.begin(), result.end(), 0 ); + //========substep 1: search foreach match its neighbors and verify if they are gvld-consistent ============// + for( int it1 = 0; it1 < size - 1; it1++ ) + { + if( valide[ it1 ] ) + { + size_t a1 = matches[ it1 ].first, b1 = matches[ it1 ].second; + + for( int it2 = it1 + 1; it2 < size; it2++ ) + if(valide[ it2 ]) + { + size_t a2 = matches[ it2 ].first, b2 = matches[ it2 ].second; + + bool bOk = false; + if( bPrecomputedDist ) + bOk = ( dist1( a1, a2 ) > min_dist && dist2( b1, b2 ) > min_dist + && ( dist1( a1, a2 ) < range1 || dist2( b1, b2 ) < range2 ) ); + else + bOk = ( point_distance( F1[ a1 ], F1[ a2 ] ) > min_dist && point_distance( F2[ b1 ], F2[ b2 ] ) > min_dist && + ( point_distance( F1[ a1 ], F1[ a2 ] ) < range1 || point_distance( F2[ b1 ], F2[ b2 ] ) < range2 ) ); + if( bOk ) + { + if( E( it1, it2 ) == -1 ) + { //update E ifunknow + E( it1, it2 ) = -2; + E( it2, it1 ) = -2; + + if( !kvldParameters.geometry || consistent( F1[ a1 ], F1[ a2 ], F2[ b1 ], F2[ b2 ] ) < distance_thres ) + { + VLD vld1( Chaine1, F1[ a1 ], F1[ a2 ] ); + VLD vld2( Chaine2, F2[ b1 ], F2[ b2 ] ); + //vld1.test(); + double error = vld1.difference( vld2 ); + //cout<= 0 ) + { + result[ it1 ] += 1; + result[ it2 ] += 1; + scoretable[ it1 ] += double( E( it1, it2 ) ); + scoretable[ it2 ] += double( E( it1, it2 ) ); + if( result[ it1 ] >= max_connection ) + break; + } + } + } + } + } + + //========substep 2: remove false matches by K gvld-consistency criteria ============// + for( int it = 0; it < size; it++ ) + { + if( valide[ it ] && result[ it ] < kvldParameters.K ) + { + valide[ it ] = false; + change = true; + } + } + //========substep 3: remove multiple matches to a same point by keeping the one with the best average gvld-consistency score ============// + if( uniqueMatch ) + for( int it1 = 0; it1 < size - 1; it1++ ) + if( valide[ it1 ]) { + size_t a1 = matches[ it1 ].first; + size_t b1 = matches[ it1 ].second; + + for( int it2 = it1 + 1; it2 < size; it2++ ) + if( valide[ it2 ] ) + { + size_t a2 = matches[ it2 ].first; + size_t b2 = matches[ it2 ].second; + + if( a1 == a2 || b1 == b2 + || ( F1[ a1 ].x() == F1[ a2 ].x() && F1[ a1 ].y() == F1[ a2 ].y() && + ( F2[ b1 ].x() != F2[ b2 ].x() || F2[ b1 ].y() != F2[ b2 ].y() ) ) + || ( ( F1[ a1 ].x() != F1[ a2 ].x() || F1[ a1 ].y() != F1[ a2 ].y() ) && + F2[ b1 ].x() == F2[ b2 ].x() && F2[ b1 ].y() == F2[ b2 ].y() ) ) + { + //cardinal comparison + if( result[ it1 ] > result[ it2 ] ) + { + valide[ it2 ] = false; + change = true; + } + else if( result[ it1 ] < result[ it2 ] ) + { + valide[ it1 ] = false; + change = true; + } + else if( result[ it1 ] == result[ it2 ] ) + { + //score comparison + if( scoretable[ it1 ] > scoretable[ it2 ] ) + { + valide[ it1 ] = false; + change = true; + } + else if( scoretable[ it1 ] < scoretable[ it2 ] ) + { + valide[ it2 ] = false; + change = true; + } + } + } + } + } + //========substep 4: ifgeometric verification is set, re-score matches by geometric-consistency, and remove poorly scored ones ============================// + if( uniqueMatch && kvldParameters.geometry ) + { + for( int i = 0; i < size; i++ ) + scoretable[ i ]=0; + + vector< bool > switching; + for( int i = 0; i < size; i++ ) + switching.push_back( false ); + + for( int it1 = 0; it1 < size; it1++ ) + { + if( valide[ it1 ] ) + { + size_t a1 = matches[ it1 ].first, b1 = matches[ it1 ].second; + float index = 0.0f; + int good_index = 0; + for( int it2 = 0; it2 < size; it2++ ) + { + if( it1 != it2 && valide[ it2 ] ) + { + size_t a2 = matches[ it2 ].first; + size_t b2 = matches[ it2 ].second; + + bool bOk = false; + if( bPrecomputedDist ) + bOk = ( dist1( a1, a2 ) > min_dist && dist2( b1, b2 ) > min_dist && + ( dist1( a1, a2 ) < range1 || dist2( b1, b2 ) < range2 ) ); + else + bOk = ( point_distance( F1[ a1 ], F1[ a2 ] ) > min_dist && point_distance( F2[ b1 ], F2[ b2 ] ) > min_dist && + ( point_distance( F1[ a1 ], F1[ a2 ] ) < range1 || point_distance( F2[ b1 ], F2[ b2 ] ) < range2 ) ); + if( bOk ) + { + float d = consistent( F1[ a1 ], F1[ a2 ], F2[ b1 ], F2[ b2 ] ); + scoretable[ it1 ] += d; + index += 1; + if( d < distance_thres ) + good_index++; + } + } + } + scoretable[ it1 ] /= index; + if( good_index < 0.3f * float( index ) && scoretable[ it1 ] > 1.2 ) + { + switching[ it1 ] = true; + change = true; + } + } + } + for( int it1 = 0; it1 < size; it1++ ) + if( switching[ it1 ] ) + valide[ it1 ] = false; + } + } +// cout<& vec_F1, + const vector< openMVG::SIOPointFeature >& vec_F2, + const vector< Pair >& vec_matches, + const vector< Pair >& vec_matchesFiltered, + const vector< double >& vec_score ) +{ +//========features + ofstream feature1( ( output + "Detectors1.txt" ).c_str() ); + if( !feature1.is_open() ) + cout << "error while writing Features1.txt" << endl; + + feature1 << vec_F1.size() << endl; + for( vector< openMVG::SIOPointFeature >::const_iterator it = vec_F1.begin(); + it != vec_F1.end(); + it++ ) + { + writeDetector( feature1, ( *it ) ); + } + feature1.close(); + + ofstream feature2( ( output + "Detectors2.txt" ).c_str() ); + if( !feature2.is_open() ) + cout << "error while writing Features2.txt" << endl; + feature2 << vec_F2.size() << endl; + for( vector< openMVG::SIOPointFeature >::const_iterator it = vec_F2.begin(); + it != vec_F2.end(); + it++ ) + { + writeDetector( feature2, ( *it ) ); + } + feature2.close(); + +//========matches + ofstream initialmatches( ( output + "initial_matches.txt" ).c_str() ); + if( !initialmatches.is_open() ) + cout << "error while writing initial_matches.txt" << endl; + initialmatches << vec_matches.size() << endl; + for( vector< Pair >::const_iterator it = vec_matches.begin(); it != vec_matches.end(); it++ ) + { + initialmatches << it->first << " " << it->second << endl; + } + initialmatches.close(); + +//==========kvld filtered matches + ofstream filteredmatches( ( output + "kvld_matches.txt" ).c_str() ); + if( !filteredmatches.is_open() ) + cout << "error while writing kvld_filtered_matches.txt" << endl; + + filteredmatches << vec_matchesFiltered.size() << endl; + for( vector< Pair >::const_iterator it = vec_matchesFiltered.begin(); + it != vec_matchesFiltered.end(); + it++ ) + { + filteredmatches << it->first << " " << it->second << endl; + + } + filteredmatches.close(); + +//====== KVLD score of matches + ofstream kvldScore( ( output + "kvld_matches_score.txt" ).c_str() ); + if( !kvldScore.is_open() ) + cout << "error while writing kvld_matches_score.txt" << endl; + + for( vector< double >::const_iterator it = vec_score.begin(); + it != vec_score.end(); + it++ ) + { + filteredmatches << *it << endl; + } + kvldScore.close(); +} + +} + diff --git a/src/openMVG/matching/kvld/kvld.h b/src/openMVG/matching/kvld/kvld.h new file mode 100644 index 0000000000..223708cb9c --- /dev/null +++ b/src/openMVG/matching/kvld/kvld.h @@ -0,0 +1,193 @@ +#ifndef KVLD_H +#define KVLD_H +/** @Main KVLD algorithm implementation + ** @Containing scale image pyramid, VLD structure and KVLD algorithm + ** @author Zhe Liu + **/ + +/* +Copyright (C) 2011-12 Zhe Liu and Pierre Moulon. +All rights reserved. + +This file is part of the KVLD library and is made available under +the terms of the BSD license (see the COPYING file). +*/ + +#pragma once +#include +#include +#include +#include +#include "algorithm.h" +#include +#include + +//Parameters concerning speed and performance + const bool uniqueMatch = true;//if activated, a point can be matched to only one point in the other image. Note: if false, it also desactivate partially geometric verification + const double juge = 0.35; + const size_t max_connection = 20; + const double distance_thres = 0.5; + const float min_dist = 10; + const float maxContrast = 300.0f; + +//===inner parameters of VLD, usually not to change + const int dimension = 10; //number of simplified SIFT-like chaines used in a single vld + const int subdirection = 8; // number of bins in a SIFT-like chaine's histogramme + const int binNum = 24;//number of bins for SIFT-like chaines' main direction. Must be a pair number + +//===== initialize parameters for a KVLD process ====// +// inlierRate: the minimum rate down to which the KVLD should be reprocessed with a lower number of inlierRate, initially set to 0.04 +// K: the minumum number of gvld-consistent (or vld-consistent) neighbors to select a match as correct one +// geometry: if true, KVLD will also take geometric verification into account. c.f. paper +// if false, KVLD execute a pure photometric verification +struct KvldParameters +{ + float inlierRate; + size_t K; + bool geometry; + KvldParameters(): inlierRate( 0.04 ), K( 3 ), geometry( true ){}; +}; + +//====== Pyramide of scale images ======// +// elements as angles[0] and magnitudes[0] presents orientations and gradient normes of the original scale (biggest), and the following elements +// are scaled down by a step of sqrt(2.0) in size +// +// angles: strore orientations of pixels of each scale image into a vector of images, which varies from 0 to 2*PI for each pixel +// magnitudes: strore gradient normes of pixels of each scale image into a vector of images +struct ImageScale +{ + std::vector< Image< float > > angles; + std::vector< Image< float > > magnitudes; + std::vector< double > ratios; + double radius_size; + double step; + + ImageScale( const Image< float >& I,double r = 5.0 ); + int getIndex( const double r )const; + +private: + void GradAndNorm( const Image< float >& I, Image< float >& angle, Image< float >& m ); +}; + +//====== VLD structures ======// +class VLD +{ + double contrast; + float distance; + + float begin_point[ 2 ]; + float end_point[ 2 ]; + + Eigen::Matrix< int, dimension, 1 > principleAngle; //relative angle + Eigen::Matrix< double, dimension, 1 > weight; + Eigen::Matrix< double, dimension * subdirection, 1 > descriptor;//relative angle + +public: + inline double get_contrast()const{ return contrast; } + //====================constructors=====================// + template< typename T > + VLD( const ImageScale& series, T const& P1, T const& P2 ); +//=========================================class functions==============================================// + inline double get_orientation()const + { + float dy = end_point[ 1 ] - begin_point[ 1 ]; + float dx = end_point[ 0 ] - begin_point[ 0 ]; + float angle; + anglefrom( dx, dy, angle ); + return angle; + } + inline double difference( const VLD& vld2 )const + { + double diff[ 2 ]; + diff[ 0 ] = 0; + diff[ 1 ] = 0; + + if( contrast > 300 || vld2.contrast > 300 || contrast <= 0 || vld2.contrast <=0 ) + return 128; + + for( int i = 0; i < dimension; i++ ) + { + for( int j = 0; j < subdirection; j++ ) + {// term of descriptor + diff[ 0 ] += abs( descriptor[ i * subdirection + j ] - vld2.descriptor[ i * subdirection + j ] ); + } + //term of main SIFT like orientation + diff[ 1 ] += std::min( abs( principleAngle[ i ] - vld2.principleAngle[ i ] ), + binNum - abs( principleAngle[ i ] - vld2.principleAngle[ i ] ) ) * ( weight[ i ] + vld2.weight[ i ] );// orientation term + } + + diff[ 0 ] *= 0.36; + diff[ 1 ] *= 0.64 / ( binNum ); + //std::cout<<"diff = "< E = libNumerics::matrix::ones(matchesPair.size(),matchesPair.size())*(-1); +// +//valide: indices of whether the i th match in the initial match list is selected, for illustration reason, it has been externalized as an input of KVLD. it should be initialized to be a +// matches.size vector with all equal to true. e.g. std::vector valide(size, true); +// +//kvldParameters: container of miminum inlier rate, the value of K (=3 initially) and geometric verification flag (true initially) + +float KVLD( const Image< float >& I1, + const Image< float >& I2, + std::vector< openMVG::SIOPointFeature >& F1, + std::vector< openMVG::SIOPointFeature >& F2, + const std::vector< Pair >& matches, + std::vector< Pair >& matchesFiltered, + std::vector< double >& score, + openMVG::Mat& E, + std::vector< bool >& valide, + KvldParameters& kvldParameters ); + + +//====================KVLD interface======================// +void writeResult( const std::string output, + const std::vector< openMVG::SIOPointFeature >& F1, + const std::vector< openMVG::SIOPointFeature >& F2, + const std::vector< Pair >& matches, + const std::vector< Pair >& matchesFiltered, + const std::vector< double >& score ); + + +#endif //KVLD_H diff --git a/src/openMVG_Samples/CMakeLists.txt b/src/openMVG_Samples/CMakeLists.txt index bd14504191..279dfe875b 100644 --- a/src/openMVG_Samples/CMakeLists.txt +++ b/src/openMVG_Samples/CMakeLists.txt @@ -8,3 +8,5 @@ ADD_SUBDIRECTORY(undisto_Brown) ADD_SUBDIRECTORY(exifParsing) ADD_SUBDIRECTORY(sensorWidthDatabase) + +ADD_SUBDIRECTORY(kvld_filter) diff --git a/src/openMVG_Samples/kvld_filter/CMakeLists.txt b/src/openMVG_Samples/kvld_filter/CMakeLists.txt new file mode 100644 index 0000000000..c124a2f75c --- /dev/null +++ b/src/openMVG_Samples/kvld_filter/CMakeLists.txt @@ -0,0 +1,15 @@ + +MESSAGE("${GLFW_LIBRARIES}") + +ADD_DEFINITIONS(-DTHIS_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}") +ADD_EXECUTABLE(openMVG_sample_kvld kvld_filter.cpp) +TARGET_LINK_LIBRARIES(openMVG_sample_kvld + openMVG_image + openMVG_multiview + vlsift + stlplus + openMVG_kvld + ${OPENGL_gl_LIBRARY} + glfw ${GLFW_LIBRARIES} + ) + diff --git a/src/openMVG_Samples/kvld_filter/kvld_filter.cpp b/src/openMVG_Samples/kvld_filter/kvld_filter.cpp new file mode 100644 index 0000000000..75d0834bb6 --- /dev/null +++ b/src/openMVG_Samples/kvld_filter/kvld_filter.cpp @@ -0,0 +1,276 @@ + +// Copyright (c) 2012, 2013 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "openMVG/image/image.hpp" +#include "openMVG/features/features.hpp" +#include "openMVG/matching/matcher_brute_force.hpp" +#include "openMVG/matching/matcher_kdtree_flann.hpp" + +#include "openMVG_Samples/siftPutativeMatches/two_view_matches.hpp" + +using namespace openMVG::matching; + +#include "patented/sift/SIFT.hpp" +#include "third_party/stlplus3/filesystemSimplified/file_system.hpp" + +#include "openMVG/multiview/solver_homography_kernel.hpp" +#include "openMVG/multiview/conditioning.hpp" + +using namespace openMVG; + +#include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" +#include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp" + +using namespace openMVG::robust; + +#include "third_party/vectorGraphics/svgDrawer.hpp" + +using namespace svg; + +#include +#include +using namespace std; + +#include "openMVG/matching/kvld/kvld.h" +#include "third_party/cmdLine/cmdLine.h" + +int main(int argc, char **argv) { + CmdLine cmd; + + std::string sImg1 = stlplus::folder_up(string(THIS_SOURCE_DIR)) + + "/imageData/StanfordMobileVisualSearch/Ace_0.png"; + std::string sImg2 = stlplus::folder_up(string(THIS_SOURCE_DIR)) + + "/imageData/StanfordMobileVisualSearch/Ace_1.png"; + std::string sOutDir = "./kvldOut"; + std::cout << sImg1 << std::endl << sImg2 << std::endl; + cmd.add( make_option('i', sImg1, "img1") ); + cmd.add( make_option('j', sImg2, "img2") ); + cmd.add( make_option('o', sOutDir, "outdir") ); + + if (argc > 1) + { + try { + if (argc == 1) throw std::string("Invalid command line parameter."); + cmd.process(argc, argv); + } catch(const std::string& s) { + std::cerr << "Usage: " << argv[0] << ' ' + << "[-i|--img1 file] " + << "[-j|--img2 file] " + << "[-o|--outdir path] " + << std::endl; + + std::cerr << s << std::endl; + return EXIT_FAILURE; + } + } + + std::cout << " You called : " < image; + Image imageL, imageR; + string jpg_filenameL = sImg1; + string jpg_filenameR = sImg2; + + ReadImage(jpg_filenameL.c_str(), &image); + Rgb2Gray(image, &imageL); + ReadImage(jpg_filenameR.c_str(), &image); + Rgb2Gray(image, &imageR); + + // Define the used descriptor (SIFT: 128 float) + typedef float descType; + typedef Descriptor SIFTDescriptor; + + // Prepare vector to store detected feature and associated descriptor + vector featsL, featsR; + vector descsL, descsR; + // Call SIFT detector + bool bOctaveMinus1 = true; + bool bRootSift = true; + SIFTDetector(imageL, featsL, descsL, bOctaveMinus1, bRootSift); + SIFTDetector(imageR, featsR, descsR, bOctaveMinus1, bRootSift); + + // Show both images side by side + { + Image concat; + ConcatH(imageL, imageR, concat); + string out_filename = "01_concat.jpg"; + out_filename = stlplus::create_filespec(sOutDir, out_filename); + WriteImage(out_filename.c_str(), concat); + } + + //- Draw features on the two images (side by side) + { + Image concat; + ConcatH(imageL, imageR, concat); + + //-- Draw features : + for (size_t i=0; i < featsL.size(); ++i ) { + const SIOPointFeature & imaA = featsL[i]; + DrawCircle(imaA.x(), imaA.y(), imaA.scale(), 255, &concat); + } + for (size_t i=0; i < featsR.size(); ++i ) { + const SIOPointFeature & imaB = featsR[i]; + DrawCircle(imaB.x()+imageL.Width(), imaB.y(), imaB.scale(), 255, &concat); + } + string out_filename = "02_features.jpg"; + out_filename = stlplus::create_filespec(sOutDir, out_filename); + WriteImage(out_filename.c_str(), concat); + } + + std::vector vec_PutativeMatches; + //-- Perform matching -> find Nearest neighbor, filtered with Distance ratio + { + // Define the matcher (ANN) + typedef ArrayMatcher_Kdtree_Flann MatcherT; + + // LoweRatio quite high in order to obtain contamined data. Squared due to squared metric + getPutativesMatches(descsL, descsR, Square(0.8), vec_PutativeMatches); + + // Draw correspondences after Nearest Neighbor ratio filter + svgDrawer svgStream( imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height())); + svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height()); + svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width()); + for (size_t i = 0; i < vec_PutativeMatches.size(); ++i) { + //Get back linked feature, draw a circle and link them by a line + const SIOPointFeature & L = featsL[vec_PutativeMatches[i]._i]; + const SIOPointFeature & R = featsR[vec_PutativeMatches[i]._j]; + svgStream.drawLine(L.x(), L.y(), R.x()+imageL.Width(), R.y(), svgStyle().stroke("green", 2.0)); + svgStream.drawCircle(L.x(), L.y(), L.scale(), svgStyle().stroke("yellow", 2.0)); + svgStream.drawCircle(R.x()+imageL.Width(), R.y(), R.scale(),svgStyle().stroke("yellow", 2.0)); + } + string out_filename = "03_siftMatches.svg"; + out_filename = stlplus::create_filespec(sOutDir, out_filename); + ofstream svgFile( out_filename.c_str() ); + svgFile << svgStream.closeSvgFile().str(); + svgFile.close(); + } + + + //K-VLD filter + std::vector vec_KVLDMatches; + + Image imgA (imageL.GetMat().cast()); + Image imgB (imageR.GetMat().cast()); + + std::vector matchesFiltered; + std::vector matchesPair; + + for (std::vector::const_iterator i_match = vec_PutativeMatches.begin(); + i_match != vec_PutativeMatches.end(); ++i_match){ + matchesPair.push_back(std::make_pair(i_match->_i, i_match->_j)); + } + std::vector vec_score; + + //In order to illustrate the gvld(or vld)-consistant neighbors, + // the following two parameters has been externalized as inputs of the function KVLD. + openMVG::Mat E = openMVG::Mat::Ones(vec_PutativeMatches.size(), vec_PutativeMatches.size())*(-1); + // gvld-consistancy matrix, intitialized to -1, >0 consistancy value, -1=unknow, -2=false + std::vector valide(vec_PutativeMatches.size(), true);// indices of match in the initial matches, if true at the end of KVLD, a match is kept. + + size_t it_num=0; + KvldParameters kvldparameters; // initial parameters of KVLD + while (it_num < 5 && + kvldparameters.inlierRate > KVLD(imgA, imgB, featsL, featsR, matchesPair, matchesFiltered, vec_score,E,valide,kvldparameters)) { + kvldparameters.inlierRate /= 2; + //std::cout<<"low inlier rate, re-select matches with new rate="< vec_FilteredMatches; + for (std::vector::const_iterator i_matchFilter = matchesFiltered.begin(); + i_matchFilter != matchesFiltered.end(); ++i_matchFilter){ + vec_FilteredMatches.push_back(IndMatch(i_matchFilter->first, i_matchFilter->second)); + } + + //Print K-VLD consistent matches + { + svgDrawer svgStream(imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height())); + + // ".svg" + svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height()); + svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width()); + + + for (int it1=0; it1=0){ + + const SIOPointFeature & l1 = featsL[matchesPair[it1].first]; + const SIOPointFeature & r1 = featsR[matchesPair[it1].second]; + + const SIOPointFeature & l2 = featsL[matchesPair[it2].first]; + const SIOPointFeature & r2 = featsR[matchesPair[it2].second]; + + // Compute the width of the current VLD segment + float L = (l1.coords() - l2.coords()).norm(); + float width = std::max(1.f, L / (dimension+1.f)); + + // ".svg" + svgStream.drawLine(l1.x(), l1.y(), l2.x(), l2.y(), svgStyle().stroke("yellow", width)); + svgStream.drawLine(r1.x() + imageL.Width(), r1.y(), r2.x() + imageL.Width(), r2.y(), svgStyle().stroke("yellow", width)); + + } + } + } + string out_filename = "05_KVLD_Matches.svg"; + out_filename = stlplus::create_filespec(sOutDir, out_filename); + ofstream svgFile( out_filename.c_str() ); + svgFile << svgStream.closeSvgFile().str(); + svgFile.close(); + } + + + { + //Print keypoints kept by K-VLD + svgDrawer svgStream(imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height())); + + // ".svg" + svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height()); + svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width()); + + for (int it=0; it Date: Mon, 30 Dec 2013 15:03:08 +0100 Subject: [PATCH 07/68] fix compilation K-VLD. #51 --- src/openMVG/matching/kvld/kvld.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/openMVG/matching/kvld/kvld.cpp b/src/openMVG/matching/kvld/kvld.cpp index fd9567d89c..9a33fce1ab 100644 --- a/src/openMVG/matching/kvld/kvld.cpp +++ b/src/openMVG/matching/kvld/kvld.cpp @@ -509,5 +509,3 @@ void writeResult( const string output, kvldScore.close(); } -} - From bfc9db114cdc0677751b9bf1b537eaa906f25413 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 30 Dec 2013 15:09:53 +0100 Subject: [PATCH 08/68] Add stlplus timer module. #52 --- src/third_party/stlplus3/CMakeLists.txt | 18 ++- .../stlplus3/subsystems/dprintf.cpp | 92 +++++++++++++ .../stlplus3/subsystems/dprintf.hpp | 122 +++++++++++++++++ .../stlplus3/subsystems/subsystems_fixes.hpp | 42 ++++++ src/third_party/stlplus3/subsystems/time.cpp | 129 ++++++++++++++++++ src/third_party/stlplus3/subsystems/time.hpp | 55 ++++++++ src/third_party/stlplus3/subsystems/timer.cpp | 60 ++++++++ src/third_party/stlplus3/subsystems/timer.hpp | 56 ++++++++ 8 files changed, 573 insertions(+), 1 deletion(-) create mode 100644 src/third_party/stlplus3/subsystems/dprintf.cpp create mode 100644 src/third_party/stlplus3/subsystems/dprintf.hpp create mode 100644 src/third_party/stlplus3/subsystems/subsystems_fixes.hpp create mode 100644 src/third_party/stlplus3/subsystems/time.cpp create mode 100644 src/third_party/stlplus3/subsystems/time.hpp create mode 100644 src/third_party/stlplus3/subsystems/timer.cpp create mode 100644 src/third_party/stlplus3/subsystems/timer.hpp diff --git a/src/third_party/stlplus3/CMakeLists.txt b/src/third_party/stlplus3/CMakeLists.txt index cfd2cb607c..d5d80c2d6a 100644 --- a/src/third_party/stlplus3/CMakeLists.txt +++ b/src/third_party/stlplus3/CMakeLists.txt @@ -1 +1,17 @@ -ADD_SUBDIRECTORY(filesystemSimplified) \ No newline at end of file +project(stlplus C CXX) + +FILE(GLOB LIBSLTPLUS_HPP_FILESYSTEM "${CMAKE_CURRENT_SOURCE_DIR}/filesystemSimplified/*.hpp" ) +FILE(GLOB LIBSLTPLUS_CPP_FILESYSTEM "${CMAKE_CURRENT_SOURCE_DIR}/filesystemSimplified/*.cpp" ) + +FILE(GLOB LIBSLTPLUS_HPP_SUBSYSTEMS "${CMAKE_CURRENT_SOURCE_DIR}/subsystems/*.hpp" ) +FILE(GLOB LIBSLTPLUS_CPP_SUBSYSTEMS "${CMAKE_CURRENT_SOURCE_DIR}/subsystems/*.cpp") + +LIST(APPEND LIBSLTPLUS_SRCS + ${LIBSLTPLUS_HPP_FILESYSTEM} ${LIBSLTPLUS_CPP_FILESYSTEM} + ${LIBSLTPLUS_HPP_SUBSYSTEMS} ${LIBSLTPLUS_CPP_SUBSYSTEMS}) + +INCLUDE_DIRECTORIES(./filesystemSimplified ./subsystems) +ADD_LIBRARY(stlplus ${LIBSLTPLUS_SRCS}) + + + diff --git a/src/third_party/stlplus3/subsystems/dprintf.cpp b/src/third_party/stlplus3/subsystems/dprintf.cpp new file mode 100644 index 0000000000..d9a9e4f0a2 --- /dev/null +++ b/src/third_party/stlplus3/subsystems/dprintf.cpp @@ -0,0 +1,92 @@ +//////////////////////////////////////////////////////////////////////////////// + +// Author: Andy Rushton +// Copyright: (c) Southampton University 1999-2004 +// (c) Andy Rushton 2004 onwards +// License: BSD License, see ../docs/license.html + +// Notes: + +// Feb 2007: Rewritten in terms of platform-specific fixes to the +// buffer-overflow problem. Using native functions for this has the added +// benefit of giving access to other features of the C-runtime such as Unicode +// support. + +//////////////////////////////////////////////////////////////////////////////// + +#include "dprintf.hpp" +#include +#include +#include +#include +#include + +//////////////////////////////////////////////////////////////////////////////// + +namespace stlplus +{ + +//////////////////////////////////////////////////////////////////////////////// + + int vdprintf(std::string& formatted, const char* format, va_list args) + { +#ifdef MSWINDOWS + int length = 0; + char* buffer = 0; + for(int buffer_length = 256; ; buffer_length*=2) + { + buffer = (char*)malloc(buffer_length); + if (!buffer) return -1; + length = _vsnprintf(buffer, buffer_length-1, format, args); + if (length >= 0) + { + buffer[length] = 0; + formatted += std::string(buffer); + free(buffer); + break; + } + free(buffer); + } + return length; +#else + char* buffer = 0; + int length = vasprintf(&buffer, format, args); + if (!buffer) return -1; + if (length >= 0) + formatted += std::string(buffer); + free(buffer); + return length; +#endif + } + + int dprintf(std::string& formatted, const char* format, ...) + { + va_list args; + va_start(args, format); + int result = vdprintf(formatted, format, args); + va_end(args); + return result; + } + + std::string vdformat(const char* format, va_list args) throw(std::invalid_argument) + { + std::string formatted; + int length = vdprintf(formatted, format, args); + if (length < 0) throw std::invalid_argument("dprintf"); + return formatted; + } + + std::string dformat(const char* format, ...) throw(std::invalid_argument) + { + std::string formatted; + va_list args; + va_start(args, format); + int length = vdprintf(formatted, format, args); + va_end(args); + if (length < 0) throw std::invalid_argument("dprintf"); + return formatted; + } + +//////////////////////////////////////////////////////////////////////////////// + +} // end namespace stlplus diff --git a/src/third_party/stlplus3/subsystems/dprintf.hpp b/src/third_party/stlplus3/subsystems/dprintf.hpp new file mode 100644 index 0000000000..3e4aa92b6f --- /dev/null +++ b/src/third_party/stlplus3/subsystems/dprintf.hpp @@ -0,0 +1,122 @@ +#ifndef STLPLUS_DPRINTF +#define STLPLUS_DPRINTF +//////////////////////////////////////////////////////////////////////////////// + +// Author: Andy Rushton +// Copyright: (c) Southampton University 1999-2004 +// (c) Andy Rushton 2004 onwards +// License: BSD License, see ../docs/license.html + +// Provides an sprintf-like function acting on STL strings. The 'd' in dprintf +// stands for "dynamic" in that the string is a dynamic string whereas a char* +// buffer would be static (in size that is, not static in C terms). + +// The obvious solution to the problem of in-memory formatted output is to use +// sprintf(), but this is a potentially dangerous operation since it will quite +// happily charge off the end of the string it is printing to and thereby +// corrupt memory. This kind of buffer-overflow vulnerability is the source of +// most security failures exploited by virus-writers. It means that sprintf +// should *never* be used and should be made obsolete. + +// In any case, using arbitrary-sized fixed-length buffers is not part of any +// quality-orientated design philosophy. + +// Most operating systems now have a safe version of sprintf, but this is +// non-standard. The functions in this file are platform-independent interfaces +// to the underlying safe implementation. + +// I would like to make this set of functions obsolete too, since I believe the +// C runtime should be deprecated in favour of C++ runtime which uses dynamic +// strings and can handle exceptions. However, there is as yet no C++ +// equivalent functionality to some of the string-handling available through +// the printf-like functions, so it has to stay for now. + +// int dprintf (std::string& buffer, const char* format, ...); + +// Formats the message by appending to the std::string buffer according to +// the formatting codes in the format string. The return int is the number +// of characters generated by this call, i.e. the increase in the length of +// the std::string. + +// int vdprintf (std::string& buffer, const char* format, va_list args); + +// As above, but using a pre-initialised va_args argument list. Useful for +// nesting dprintf calls within variable argument functions. + +// std::string dformat (const char* format, ...); + +// Similar to dprintf() above, except the result is formatted into a new +// std::string which is returned by the function. Very useful for inline +// calls within an iostream expression. + +// e.g. cout << "Total: " << dformat("%6i",t) << endl; + +// std::string vdformat (const char* format, va_list); + +// As above, but using a pre-initialised va_args argument list. Useful for nesting +// dformat calls within variable argument functions. + +// The format string supports the following format codes as in the C runtime library: + +// % [ flags ] [ field ] [ . precision ] [ modifier ] [ conversion ] + +// flags: +// - - left justified +// + - print sign for +ve numbers +// ' ' - leading space where + sign would be +// 0 - leading zeros to width of field +// # - alternate format + +// field: +// a numeric argument specifying the field width - default = 0 +// * means take the next va_arg as the field width - if negative then left justify + +// precision: +// a numeric argument the meaning of which depends on the conversion - +// - %s - max characters from a string - default = strlen() +// - %e, %f - decimal places to be displayed - default = 6 +// - %g - significant digits to be displayed - default = 6 +// - all integer conversions - minimum digits to display - default = 0 +// * means take the next va_arg as the field width - if negative then left justify + +// modifier: +// h - short or unsigned short +// l - long or unsigned long +// L - long double + +// conversions: +// d, i - short/int/long as decimal +// u - short/int/long as unsigned decimal +// o - short/int/long as unsigned octal - # adds leading 0 +// x, X - short/int/long as unsigned hexadecimal - # adds leading 0x +// c - char +// s - char* +// f - double/long double as fixed point +// e, E - double/long double as floating point +// g, G - double/long double as fixed point/floating point depending on value +// p - void* as unsigned hexadecimal +// % - literal % +// n - int* as recipient of length of formatted string so far + +//////////////////////////////////////////////////////////////////////////////// +#include "portability_fixes.hpp" +#include +#include +#include + +namespace stlplus +{ + + // format by appending to a string and return the increase in length + // if there is an error, return a negative number and leave the string unchanged + int dprintf (std::string& formatted, const char* format, ...); + int vdprintf (std::string& formatted, const char* format, va_list args); + + // format into a new string and return the result + // if there is an error, throw an exception + std::string dformat (const char* format, ...) throw(std::invalid_argument); + std::string vdformat (const char* format, va_list) throw(std::invalid_argument); + +} // end namespace stlplus + +#endif diff --git a/src/third_party/stlplus3/subsystems/subsystems_fixes.hpp b/src/third_party/stlplus3/subsystems/subsystems_fixes.hpp new file mode 100644 index 0000000000..bcaf8b9e3b --- /dev/null +++ b/src/third_party/stlplus3/subsystems/subsystems_fixes.hpp @@ -0,0 +1,42 @@ +#ifndef STLPLUS_SUBSYSTEMS_FIXES +#define STLPLUS_SUBSYSTEMS_FIXES +//////////////////////////////////////////////////////////////////////////////// + +// Author: Andy Rushton +// Copyright: (c) Southampton University 1999-2004 +// (c) Andy Rushton 2004 onwards +// License: BSD License, see ../docs/license.html + +// Contains work arounds for OS or Compiler specific problems + +//////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////// +// Unnecessary compiler warnings +//////////////////////////////////////////////////////////////////////////////// + +#ifdef _MSC_VER +// Microsoft Visual Studio +// shut up the following irritating warnings +// 4786 - VC6, identifier string exceeded maximum allowable length and was truncated (only affects debugger) +// 4305 - VC6, identifier type was converted to a smaller type +// 4503 - VC6, decorated name was longer than the maximum the compiler allows (only affects debugger) +// 4309 - VC6, type conversion operation caused a constant to exceeded the space allocated for it +// 4290 - VC6, C++ exception specification ignored +// 4800 - VC6, forcing value to bool 'true' or 'false' (performance warning) +// 4675 - VC7.1, "change" in function overload resolution _might_ have altered program +// 4996 - VC8, 'xxxx' was declared deprecated +#pragma warning(disable: 4786 4305 4503 4309 4290 4800 4675 4996) +#endif + +#ifdef __BORLANDC__ +// Borland +// Shut up the following irritating warnings +// 8026 - Functions with exception specifications are not expanded inline +// 8027 - Functions with xxx are not expanded inline +#pragma warn -8026 +#pragma warn -8027 +#endif + +//////////////////////////////////////////////////////////////////////////////// +#endif diff --git a/src/third_party/stlplus3/subsystems/time.cpp b/src/third_party/stlplus3/subsystems/time.cpp new file mode 100644 index 0000000000..7ed646da94 --- /dev/null +++ b/src/third_party/stlplus3/subsystems/time.cpp @@ -0,0 +1,129 @@ +//////////////////////////////////////////////////////////////////////////////// + +// Author: Andy Rushton +// Copyright: (c) Southampton University 1999-2004 +// (c) Andy Rushton 2004 onwards +// License: BSD License, see ../docs/license.html + +//////////////////////////////////////////////////////////////////////////////// +#include "time.hpp" +#include "dprintf.hpp" +#include +//////////////////////////////////////////////////////////////////////////////// + +namespace stlplus +{ + + //////////////////////////////////////////////////////////////////////////////// + + time_t time_now(void) + { + return time(0); + } + + time_t localtime_create(int year, int month, int day, int hour, int minute, int second) + { + tm tm_time; + tm_time.tm_year = year-1900; // years are represented as an offset from 1900, for reasons unknown + tm_time.tm_mon = month-1; // internal format represents month as 0-11, but it is friendlier to take an input 1-12 + tm_time.tm_mday = day; + tm_time.tm_hour = hour; + tm_time.tm_min = minute; + tm_time.tm_sec = second; + tm_time.tm_isdst = -1; // specify that the function should work out daylight savings + time_t result = mktime(&tm_time); + return result; + } + + int localtime_year(time_t t) + { + tm* tm_time = localtime(&t); + return tm_time->tm_year + 1900; + } + + int localtime_month(time_t t) + { + tm* tm_time = localtime(&t); + return tm_time->tm_mon + 1; + } + + int localtime_day(time_t t) + { + tm* tm_time = localtime(&t); + return tm_time->tm_mday; + } + + int localtime_hour(time_t t) + { + tm* tm_time = localtime(&t); + return tm_time->tm_hour; + } + + int localtime_minute(time_t t) + { + tm* tm_time = localtime(&t); + return tm_time->tm_min; + } + + int localtime_second(time_t t) + { + tm* tm_time = localtime(&t); + return tm_time->tm_sec; + } + + int localtime_weekday(time_t t) + { + tm* tm_time = localtime(&t); + return tm_time->tm_wday; + } + + int localtime_yearday(time_t t) + { + tm* tm_time = localtime(&t); + return tm_time->tm_yday; + } + + std::string localtime_string(time_t t) + { + tm* local = localtime(&t); + std::string result = local ? asctime(local) : "*time not available*"; + // ctime appends a newline for no apparent reason - clean up + while (!result.empty() && isspace(result[result.size()-1])) + result.erase(result.size()-1,1); + return result; + } + + std::string delaytime_string(time_t seconds) + { + unsigned minutes = (unsigned)seconds / 60; + seconds %= 60; + unsigned hours = minutes / 60; + minutes %= 60; + unsigned days = hours / 24; + hours %= 24; + unsigned weeks = days / 7; + days %= 7; + std::string result; + if (weeks > 0) + result += dformat("%dw ",weeks); + if (!result.empty() || days > 0) + result += dformat("%dd ", days); + if (!result.empty() || hours > 0) + result += dformat("%d:", hours); + if (!result.empty() || minutes > 0) + { + if (!result.empty()) + result += dformat("%02d:", minutes); + else + result += dformat("%d:", minutes); + } + if (!result.empty()) + result += dformat("%02d:", seconds); + else + result += dformat("%ds", seconds); + return result; + } + + //////////////////////////////////////////////////////////////////////////////// + +} // end namespace stlplus diff --git a/src/third_party/stlplus3/subsystems/time.hpp b/src/third_party/stlplus3/subsystems/time.hpp new file mode 100644 index 0000000000..8b6e1de3ba --- /dev/null +++ b/src/third_party/stlplus3/subsystems/time.hpp @@ -0,0 +1,55 @@ +#ifndef STLPLUS_TIME +#define STLPLUS_TIME +//////////////////////////////////////////////////////////////////////////////// + +// Author: Andy Rushton +// Copyright: (c) Southampton University 1999-2004 +// (c) Andy Rushton 2004 onwards +// License: BSD License, see ../docs/license.html + +// Simplified access to representations of time and conversions between them. +// The motivation for this package is that the low-level system calls for +// accessing time are ugly and therefore potentially error-prone. I hope that +// this interface is much simpler and therefore easier to use and more likely +// to yield first-time right programs. + +// time is represented as the built-in integer type time_t - this is the +// standard representation of system time in computerland and represents the +// number of seconds since midnight 1 Jan 1970, believe it or not. + +// Functions are provided here for converting to and from more +// human-comprehendable forms. + +//////////////////////////////////////////////////////////////////////////////// +#include "portability_fixes.hpp" +#include +#include + +namespace stlplus +{ + + // get the integer representing the time now + time_t time_now(void); + + // get the integer representing the requested time - the local time is expressed in the local timezone + time_t localtime_create(int year, int month, int day, int hour, int minute, int second); + + // extract human-centric form of the machine representation time_t + int localtime_year(time_t); // the year e.g. 1962 + int localtime_month(time_t); // the month, numbered 1-12 e.g. August = 8 + int localtime_day(time_t); // the day of the month numbered 1-31 e.g. 29 + int localtime_hour(time_t); // the hour of day numbered 0-23 + int localtime_minute(time_t); // minute past the hour numbered 0-59 + int localtime_second(time_t); // second past the minute numbered 0-59 + int localtime_weekday(time_t); // the day of the week numbered 0-6 with 0=Sunday + int localtime_yearday(time_t); // the number of days into the year + + // convert the integer representation of time to a human-readable form + std::string localtime_string(time_t); + + // convert a time delay in seconds to human-readable form + std::string delaytime_string(time_t); + +} // end namespace stlplus + +#endif diff --git a/src/third_party/stlplus3/subsystems/timer.cpp b/src/third_party/stlplus3/subsystems/timer.cpp new file mode 100644 index 0000000000..5fb6249143 --- /dev/null +++ b/src/third_party/stlplus3/subsystems/timer.cpp @@ -0,0 +1,60 @@ +//////////////////////////////////////////////////////////////////////////////// + +// Author: Andy Rushton +// Copyright: (c) Southampton University 1999-2004 +// (c) Andy Rushton 2004 onwards +// License: BSD License, see ../docs/license.html + +//////////////////////////////////////////////////////////////////////////////// +#include "timer.hpp" +#include "dprintf.hpp" +#include "time.hpp" + +//////////////////////////////////////////////////////////////////////////////// + +namespace stlplus +{ + +//////////////////////////////////////////////////////////////////////////////// + + timer::timer(void) + { + reset(); + } + + timer::~timer(void) + { + } + + void timer::reset(void) + { + m_clock = clock(); + m_time = time(0); + } + + float timer::cpu(void) const + { + return ((float)(clock() - m_clock)) / ((float)CLOCKS_PER_SEC); + } + + float timer::elapsed(void) const + { + return difftime(time(0), m_time); + } + + /*std::string timer::text(void) const + { + return dformat("%4.2fs CPU, %s elapsed", cpu(), delaytime_string(time(0)-m_time).c_str()); + }*/ + +//////////////////////////////////////////////////////////////////////////////// + + std::ostream& operator << (std::ostream& str, const timer& t) + { + //return str << t.text(); + return str << t.cpu() << " s elapsed"; + } + +//////////////////////////////////////////////////////////////////////////////// + +} // end namespace stlplus diff --git a/src/third_party/stlplus3/subsystems/timer.hpp b/src/third_party/stlplus3/subsystems/timer.hpp new file mode 100644 index 0000000000..e0a05053c0 --- /dev/null +++ b/src/third_party/stlplus3/subsystems/timer.hpp @@ -0,0 +1,56 @@ +#ifndef STLPLUS_TIMER +#define STLPLUS_TIMER +//////////////////////////////////////////////////////////////////////////////// + +// Author: Andy Rushton +// Copyright: (c) Southampton University 1999-2004 +// (c) Andy Rushton 2004 onwards +// License: BSD License, see ../docs/license.html + +// A CPU timer encapsulated as a class. Measures the CPU time used since its +// construction and allows this cumulative time to be reported at any time. + +//////////////////////////////////////////////////////////////////////////////// +#include "subsystems_fixes.hpp" +#include +#include +#include + +namespace stlplus +{ + + //////////////////////////////////////////////////////////////////////////////// + + class timer + { + private: + clock_t m_clock; + time_t m_time; + + public: + // constructor resets the timer to zero + timer(void); + ~timer(void); + + // reset the timer to zero without destroying it + void reset(void); + + // get the elapsed time in seconds, expressed as a float + float elapsed(void) const; + // get the CPU time in seconds, expressed as a float + float cpu(void) const; + + // get a printable string representing the elapsed time and CPU time + //std::string text(void) const; + }; + + //////////////////////////////////////////////////////////////////////////////// + + // print the elapsed time and CPU time using the same representation as the text method + std::ostream& operator << (std::ostream&, const timer&); + + //////////////////////////////////////////////////////////////////////////////// + +} // end namespace stlplus + +#endif From f253a671730e16bcc0549d97cc82260c0c462315 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 30 Dec 2013 15:32:54 +0100 Subject: [PATCH 09/68] Initial implementation of a dense solver for global rotations computations from relative ones. #53 --- src/openMVG/multiview/CMakeLists.txt | 2 +- src/openMVG/multiview/rotation_averaging.hpp | 130 ++++++++++++++++++ .../multiview/rotation_averaging_test.cpp | 105 ++++++++++++++ 3 files changed, 236 insertions(+), 1 deletion(-) create mode 100644 src/openMVG/multiview/rotation_averaging.hpp create mode 100644 src/openMVG/multiview/rotation_averaging_test.cpp diff --git a/src/openMVG/multiview/CMakeLists.txt b/src/openMVG/multiview/CMakeLists.txt index 6f3317a140..6d363fc799 100644 --- a/src/openMVG/multiview/CMakeLists.txt +++ b/src/openMVG/multiview/CMakeLists.txt @@ -33,4 +33,4 @@ UNIT_TEST(openMVG solver_essential_kernel "openMVG_multiview;openMVG_multiview_t UNIT_TEST(openMVG solver_homography_kernel "openMVG_multiview;openMVG_multiview_test_data") UNIT_TEST(openMVG solver_essential_five_point "openMVG_multiview;openMVG_multiview_test_data") UNIT_TEST(openMVG solver_resection_kernel "openMVG_multiview;openMVG_multiview_test_data") - +UNIT_TEST(openMVG rotation_averaging "openMVG_multiview") diff --git a/src/openMVG/multiview/rotation_averaging.hpp b/src/openMVG/multiview/rotation_averaging.hpp new file mode 100644 index 0000000000..1e32a5b330 --- /dev/null +++ b/src/openMVG/multiview/rotation_averaging.hpp @@ -0,0 +1,130 @@ + +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_MULTIVIEW_ROTATION_AVERAGING_H_ +#define OPENMVG_MULTIVIEW_ROTATION_AVERAGING_H_ + +#include "openMVG/numeric/numeric.h" +#include +#include + +//-- +//-- Implementation related to rotation averaging. +// . Compute global rotation from a list of relative estimates. +// +//- Implementation of algorithm from Thesis titled: +//- [1] "Robust Multiview Reconstruction." +//- Author : Daniel Martinec. +//- Date : July 2, 2008. +//-- +namespace openMVG { +namespace rotation_averaging { + +// [1] 6.7.2 Consistent Rotation page 89 +// Closest Rotation Estimation R = U*transpose(V) +// approximate rotation in the Frobenius norm using SVD +inline Mat3 ClosestSVDRotationMatrix(const Mat3 & rotMat) +{ + // Closest orthogonal matrix + Eigen::JacobiSVD svd(rotMat,Eigen::ComputeFullV|Eigen::ComputeFullU); + Mat3 U = svd.matrixU(); + Mat3 V = svd.matrixV(); + return U*V.transpose(); +} + +typedef std::vector, Mat3> > vector_RelativeRotMotion; + +//-- Solve the Global Rotation matrix registration for each camera given a list +// of relative orientation using matrix parametrization +// [1] formula 6.62 page 100. Dense formulation. +//- nCamera: The number of camera to solve +//- vec_rotationEstimate: The relative rotation i->j +//- vec_ApprRotMatrix: The output global rotation + +// Minimization of the norm of: +// => || rj - Rij * ri ||= 0 +// With rj et rj the global rotation and Rij the relative rotation from i to j. +// +// Example: +// 0_______2 +// \ / +// \ / +// \ / +// 1 +// +// nCamera = 3 +// vector.add( make_pair(make_pair(0,1), R01) ); +// vector.add( make_pair(make_pair(1,2), R12) ); +// vector.add( make_pair(make_pair(0,2), R02) ); +// +static bool L2RotationAveraging( size_t nCamera, + const vector_RelativeRotMotion & vec_relativeRotEstimate, + std::vector & vec_ApprRotMatrix) +{ + using namespace std; + Mat3 Id = Mat3::Identity(); + const size_t nRotationEstimation = vec_relativeRotEstimate.size(); + //-- + // Setup the Action Matrix + //-- + // nCamera * 3 because each columns have 3 elements. + Mat A = Mat::Zero(nRotationEstimation*3, 3*nCamera); + //-- Encode constraint (6.62 Martinec Thesis page 100): + size_t cpt = 0; + for(vector_RelativeRotMotion::const_iterator + iter = vec_relativeRotEstimate.begin(); + iter != vec_relativeRotEstimate.end(); + iter++, cpt++) + { + const std::pair, Mat3> & Elem = *iter; + + //-- Encode rj - Rij * ri = 0 + int i = Elem.first.first; + int j = Elem.first.second; + + A.block<3,3>(3 * cpt, 3 * i) = - Elem.second; + A.block<3,3>(3 * cpt, 3 * j) = Id; + } + + // Solve Ax=0 => SVD + Eigen::JacobiSVD svd(A,Eigen::ComputeFullV); + const Vec & NullspaceVector0 = svd.matrixV().col(A.cols()-1); + const Vec & NullspaceVector1 = svd.matrixV().col(A.cols()-2); + const Vec & NullspaceVector2 = svd.matrixV().col(A.cols()-3); + + //-- + // Search the closest matrix : + // - From solution of SVD get back column and reconstruct Rotation matrix + // - Enforce the orthogonality constraint + // (approximate rotation in the Frobenius norm using SVD). + //-- + vec_ApprRotMatrix.clear(); + vec_ApprRotMatrix.reserve(nCamera); + for(size_t i=0; i < nCamera; ++i) + { + Mat3 Rotation; + Rotation << NullspaceVector0.segment(3 * i, 3), + NullspaceVector1.segment(3 * i, 3), + NullspaceVector2.segment(3 * i, 3); + + //-- Compute the closest SVD rotation matrix + Rotation = ClosestSVDRotationMatrix(Rotation); + vec_ApprRotMatrix.push_back(Rotation); + } + // Force R0 to be Identity + Mat3 R0T = vec_ApprRotMatrix[0].transpose(); + for(size_t i = 0; i < nCamera; ++i) { + vec_ApprRotMatrix[i] *= R0T; + } + + return true; +} + +} // namespace rotation_averaging +} // namespace openMVG + +#endif //OPENMVG_MULTIVIEW_ROTATION_AVERAGING_H_ diff --git a/src/openMVG/multiview/rotation_averaging_test.cpp b/src/openMVG/multiview/rotation_averaging_test.cpp new file mode 100644 index 0000000000..ecf4756ef8 --- /dev/null +++ b/src/openMVG/multiview/rotation_averaging_test.cpp @@ -0,0 +1,105 @@ +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "CppUnitLite/TestHarness.h" +#include "openMVG/multiview/rotation_averaging.hpp" +#include "openMVG/multiview/essential.hpp" +#include "testing/testing.h" + +#include +#include +#include +#include + +using namespace openMVG; +using namespace rotation_averaging; + +TEST ( rotation_averaging, ClosestSVDRotationMatrix ) +{ + Mat3 rotx = RotationAroundX(0.3); + + Mat3 Approximative_rotx = rotation_averaging::ClosestSVDRotationMatrix(rotx); + + // Check that SVD have rebuilt the matrix correctly + EXPECT_MATRIX_NEAR( rotx, Approximative_rotx, 1e-8); + // Check the Frobenius distance between the approximated rot matrix and the GT + EXPECT_NEAR( 0.0, FrobeniusDistance( rotx, Approximative_rotx), 1e-8); + // Check that the Matrix is a rotation matrix (determinant == 1) + EXPECT_NEAR( 1.0, Approximative_rotx.determinant(), 1e-8); +} + + +TEST ( rotation_averaging, ClosestSVDRotationMatrixNoisy ) +{ + Mat3 rotx = RotationAroundX(0.3); + + //-- Set a little of noise in the rotMatrix : + rotx(2,2) -= 0.02; + Mat3 Approximative_rotx = rotation_averaging::ClosestSVDRotationMatrix(rotx); + + // Check the Frobenius distance between the approximated rot matrix and the GT + CHECK( FrobeniusDistance( rotx, Approximative_rotx) < 0.02); + // Check that the Matrix is a rotation matrix (determinant == 1) + EXPECT_NEAR( 1.0, Approximative_rotx.determinant(), 1e-8); +} + +// Rotation averaging in a triplet: +// 0_______2 +// \ / +// \ / +// \ / +// 1 +TEST ( rotation_averaging, RotationLeastSquare_3_Camera) +{ + using namespace std; + + //-- + // Setup 3 camera that have a relative orientation of 120° + // Set Z axis as UP Vector for the rotation + // They are in the same plane and looking in O={0,0,0} + //-- + const int nCamera = 3; + Mat3 R01 = RotationAroundZ(2.*M_PI/3.0); //120° + Mat3 R12 = RotationAroundZ(2.*M_PI/3.0); //120° + Mat3 R20 = RotationAroundZ(2.*M_PI/3.0); //120° + Mat3 Id = Mat3::Identity(); + + std::vector, Mat3> > vec_relativeRotEstimate; + vec_relativeRotEstimate.push_back( make_pair(make_pair(0,1), RotationAroundZ( 2.*M_PI/3.0))); + vec_relativeRotEstimate.push_back( make_pair(make_pair(1,2), RotationAroundZ( 2.*M_PI/3.0))); + vec_relativeRotEstimate.push_back( make_pair(make_pair(2,0), RotationAroundZ( 2.*M_PI/3.0))); + + //- Solve the global rotation estimation problem : + std::vector vec_globalR; + L2RotationAveraging(3, vec_relativeRotEstimate, vec_globalR); + EXPECT_EQ(3, vec_globalR.size()); + // Check that the loop is closing + EXPECT_MATRIX_NEAR(Mat3::Identity(), (vec_globalR[0]*vec_globalR[1]*vec_globalR[2]), 1e-8); + + //-- + // Check that the found relative rotation matrix give the expected rotation. + // -> the started relative rotation (used in the action matrix). + //// /!\ Translation are not checked they are 0 by default. + //-- + Mat3 R; + Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero(); + RelativeCameraMotion(vec_globalR[0], t0, vec_globalR[1], t1, &R, &t); + EXPECT_NEAR( 0, FrobeniusDistance( R01, R), 1e-2); + std::cout << "\n" << R << "\n\n\n"; + + RelativeCameraMotion(vec_globalR[1], t0, vec_globalR[2], t1, &R, &t); + EXPECT_NEAR( 0, FrobeniusDistance( R12, R), 1e-2); + std::cout << "\n" << R << "\n\n\n"; + + RelativeCameraMotion(vec_globalR[2], t0, vec_globalR[0], t1, &R, &t); + EXPECT_NEAR( 0, FrobeniusDistance( R20, R), 1e-2); + std::cout << "\n" << R << "\n\n\n"; +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From 59761bc14f6debb8eb9e9cbaadd12146c4b4e1e9 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 30 Dec 2013 17:21:15 +0100 Subject: [PATCH 10/68] Update the openMVG graph module with a triplet listing method. #55 --- src/openMVG/graph/CMakeLists.txt | 2 + src/openMVG/graph/connectedComponent.hpp | 4 +- src/openMVG/graph/connectedComponent_test.cpp | 2 +- src/openMVG/graph/graph_triplet_finder.hpp | 117 ++++++++ .../graph/graph_triplet_finder_test.cpp | 272 ++++++++++++++++++ 5 files changed, 394 insertions(+), 3 deletions(-) create mode 100644 src/openMVG/graph/graph_triplet_finder.hpp create mode 100644 src/openMVG/graph/graph_triplet_finder_test.cpp diff --git a/src/openMVG/graph/CMakeLists.txt b/src/openMVG/graph/CMakeLists.txt index 21142912e4..846c9ca6bd 100644 --- a/src/openMVG/graph/CMakeLists.txt +++ b/src/openMVG/graph/CMakeLists.txt @@ -1,3 +1,5 @@ UNIT_TEST(openMVG connectedComponent "lemon") +UNIT_TEST(openMVG graph_triplet_finder "lemon") + diff --git a/src/openMVG/graph/connectedComponent.hpp b/src/openMVG/graph/connectedComponent.hpp index aaf9de69d9..882e2d5ca9 100644 --- a/src/openMVG/graph/connectedComponent.hpp +++ b/src/openMVG/graph/connectedComponent.hpp @@ -12,7 +12,7 @@ namespace openMVG { -namespace graph +namespace graphUtils { /// Export node of each CC (Connected Component) in a map @@ -36,7 +36,7 @@ std::map > exportGraphToMapSubgraphs( return map_subgraphs; } -} // namespace graph +} // namespace graphUtils } // namespace openMVG #endif // OPENMVG_GRAPH_CONNECTED_COMPONENT_H_ diff --git a/src/openMVG/graph/connectedComponent_test.cpp b/src/openMVG/graph/connectedComponent_test.cpp index 9afb22ebed..eaa42a6829 100644 --- a/src/openMVG/graph/connectedComponent_test.cpp +++ b/src/openMVG/graph/connectedComponent_test.cpp @@ -100,7 +100,7 @@ TEST(exportGraphToMapSubgraphs, CC_Subgraph) { graph.addEdge(j,l); const std::map > map_subgraphs = - openMVG::graph::exportGraphToMapSubgraphs(graph); + openMVG::graphUtils::exportGraphToMapSubgraphs(graph); EXPECT_EQ(4, map_subgraphs.size()); EXPECT_EQ(5, map_subgraphs.at(0).size()); diff --git a/src/openMVG/graph/graph_triplet_finder.hpp b/src/openMVG/graph/graph_triplet_finder.hpp new file mode 100644 index 0000000000..a7e9bbd37c --- /dev/null +++ b/src/openMVG/graph/graph_triplet_finder.hpp @@ -0,0 +1,117 @@ +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_GRAPH_TRIPLET_FINDER_H +#define OPENMVG_GRAPH_TRIPLET_FINDER_H + +#include "lemon/list_graph.h" +using namespace lemon; + +#include +#include + +namespace openMVG{ +namespace graphUtils{ + +/// Simple container for tuple of three value +/// It is used to store the node id of triplets of a graph. +struct Triplet +{ + Triplet(size_t ii, size_t jj, size_t kk) + :i(ii), j(jj), k(kk) + { } + size_t i,j,k; + + bool contain(const std::pair & edge) const + { + size_t It = edge.first; + size_t Jt = edge.second; + if ( (It == i || It == j || It == k ) && + (Jt == i || Jt == j || Jt == k ) && It != Jt) + return true; + else + return false; + } + + friend bool operator==(const Triplet& m1, const Triplet& m2) { + return m1.contain(std::make_pair(m2.i, m2.j)) + && m1.contain(std::make_pair(m2.i, m2.k)); + } + + friend bool operator!=(const Triplet& m1, const Triplet& m2) { + return !(m1 == m2); + } + + friend std::ostream & operator<<(std::ostream & os, const Triplet & t) + { + os << t.i << " " << t.j << " " << t.k << std::endl; + return os; + } +}; + +/// Function that return all the triplets found in a graph +/// vec_triplets must be empty. +template +bool List_Triplets(const GraphT & g, std::vector< Triplet > & vec_triplets) +{ + // Algorithm + // + //-- For each node + // - list the outgoing not visited edge + // - for each tuple of edge + // - if their end are connected + // Detected cyle of length 3 + // Mark first edge as visited + + typedef typename GraphT::OutArcIt OutArcIt; + typedef typename GraphT::NodeIt NodeIterator; + typedef typename GraphT::template EdgeMap BoolEdgeMap; + + BoolEdgeMap map_edge(g, false); // Visited edge map + + // For each nodes + for (NodeIterator itNode(g); itNode != INVALID; ++itNode) + { + // For each edges (list the not visited outgoing edges) + std::vector vec_edges; + for (OutArcIt e(g, itNode); e!=INVALID; ++e) + { + if (!map_edge[e]) // If not visited + vec_edges.push_back(e); + } + + // For all tuples look of ends of edges are linked + while(vec_edges.size()>1) + { + OutArcIt itPrev = vec_edges[0]; // For all tuple (0,Inth) + for(size_t i=1; i < vec_edges.size(); ++i) + { + // Check if the extremity is linked + typename GraphT::Arc cycleEdge = findArc(g, g.target(itPrev), g.target(vec_edges[i])); + if (cycleEdge!= INVALID && !map_edge[cycleEdge]) + { + // Elementary cycle found (make value follow a monotonic ascending serie) + double triplet[3] = { + g.id(itNode), + g.id(g.target(itPrev)), + g.id(g.target(vec_edges[i]))}; + std::sort(&triplet[0], &triplet[3]); + vec_triplets.push_back(Triplet(triplet[0],triplet[1],triplet[2])); + } + } + // Mark the current ref edge as visited + map_edge[itPrev] = true; + // remove head to list remaining tuples + vec_edges.erase(vec_edges.begin()); + } + } + return (!vec_triplets.empty()); +} + +} // namespace graphUtils +} // namespace openMVG + +#endif // OPENMVG_GRAPH_TRIPLET_FINDER_H diff --git a/src/openMVG/graph/graph_triplet_finder_test.cpp b/src/openMVG/graph/graph_triplet_finder_test.cpp new file mode 100644 index 0000000000..246d1cfed7 --- /dev/null +++ b/src/openMVG/graph/graph_triplet_finder_test.cpp @@ -0,0 +1,272 @@ +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "openMVG/graph/graph_triplet_finder.hpp" +using namespace openMVG::graphUtils; + +#include "CppUnitLite/TestHarness.h" +#include "testing/testing.h" + +#include +#include + +TEST(TripletFinder, test_no_triplet) { + + typedef lemon::ListGraph Graph; + + // a_b_c + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), c = ga.addNode(); + ga.addEdge(a,b); + ga.addEdge(b,c); + + std::vector< Triplet > vec_triplets; + + EXPECT_FALSE(List_Triplets(ga, vec_triplets)); + EXPECT_TRUE(vec_triplets.empty()); +} + +TEST(TripletFinder, test_one_triplet) { + + typedef lemon::ListGraph Graph; + + { + // + // a_b + // |/ + // c + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), c = ga.addNode(); + ga.addEdge(a,b); + ga.addEdge(a,c); + ga.addEdge(b,c); + + std::vector< Triplet > vec_triplets; + EXPECT_TRUE(List_Triplets(ga, vec_triplets)); + EXPECT_EQ(1, vec_triplets.size()); + //Check the cycle values + EXPECT_EQ(0,vec_triplets[0].i); + EXPECT_EQ(1,vec_triplets[0].j); + EXPECT_EQ(2,vec_triplets[0].k); + } + + { + // + // a_b__c + // |/ + // d + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), + c = ga.addNode(), d = ga.addNode(); + ga.addEdge(a,b); + ga.addEdge(b,c); + ga.addEdge(b,d); + ga.addEdge(c,d); + + std::vector< Triplet > vec_triplets; + EXPECT_TRUE(List_Triplets(ga, vec_triplets)); + EXPECT_EQ(1, vec_triplets.size()); + //Check the cycle values + EXPECT_EQ(1,vec_triplets[0].i); + EXPECT_EQ(2,vec_triplets[0].j); + EXPECT_EQ(3,vec_triplets[0].k); + } +} + +TEST(TripletFinder, test_two_triplet) { + + typedef lemon::ListGraph Graph; + + { + // + // a__b + // |\ | + // | \| + // c--d + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), + c = ga.addNode(), d = ga.addNode(); + + ga.addEdge(a,b); + ga.addEdge(a,c); + ga.addEdge(a,d); + ga.addEdge(c,d); + ga.addEdge(b,d); + + std::vector< Triplet > vec_triplets; + EXPECT_TRUE(List_Triplets(ga, vec_triplets)); + EXPECT_EQ(2, vec_triplets.size()); + } + + { + // + // a c + // |\ /| + // | b | + // |/ \| + // d e + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), + c = ga.addNode(), d = ga.addNode(), + e = ga.addNode(); + + ga.addEdge(a,b); + ga.addEdge(b,c); + ga.addEdge(c,e); + ga.addEdge(e,b); + ga.addEdge(b,d); + ga.addEdge(d,a); + + std::vector< Triplet > vec_triplets; + EXPECT_TRUE(List_Triplets(ga, vec_triplets)); + EXPECT_EQ(2, vec_triplets.size()); + } + + { + // + // a c + // |\ /| + // | b--f | + // |/ \| + // d e + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), + c = ga.addNode(), d = ga.addNode(), + e = ga.addNode(), f = ga.addNode(); + + ga.addEdge(a,b); + ga.addEdge(b,f); + ga.addEdge(f,c); + ga.addEdge(c,e); + ga.addEdge(e,f); + ga.addEdge(f,b); + ga.addEdge(b,d); + ga.addEdge(d,a); + + std::vector< Triplet > vec_triplets; + EXPECT_TRUE(List_Triplets(ga, vec_triplets)); + EXPECT_EQ(2, vec_triplets.size()); + } +} + + +TEST(TripletFinder, test_three_triplet) { + + typedef lemon::ListGraph Graph; + + { + // + // a b + // |\ /| + // c-d-e + // |/ + // f + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), + c = ga.addNode(), d = ga.addNode(), + e = ga.addNode(), f = ga.addNode(); + + ga.addEdge(a,c); + ga.addEdge(a,d); + ga.addEdge(c,d); + ga.addEdge(c,f); + ga.addEdge(f,d); + ga.addEdge(d,b); + ga.addEdge(b,e); + ga.addEdge(e,d); + + std::vector< Triplet > vec_triplets; + EXPECT_TRUE(List_Triplets(ga, vec_triplets)); + EXPECT_EQ(3, vec_triplets.size()); + } + + { + // + // a b--g--h + // | \ / | \/ + // | d--e | i + // | / \ | + // c f + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), + c = ga.addNode(), d = ga.addNode(), + e = ga.addNode(), f= ga.addNode(), + g = ga.addNode(), h = ga.addNode(), + i = ga.addNode(); + + ga.addEdge(a,c); + ga.addEdge(a,d); + ga.addEdge(d,c); + ga.addEdge(d,e); + ga.addEdge(e,b); + ga.addEdge(e,f); + ga.addEdge(b,f); + ga.addEdge(b,g); + ga.addEdge(g,h); + ga.addEdge(h,i); + ga.addEdge(i,g); + + std::vector< Triplet > vec_triplets; + EXPECT_TRUE(List_Triplets(ga, vec_triplets)); + EXPECT_EQ(3, vec_triplets.size()); + } + + { + // + // a---b + // |\ |\ + // | \ | \ + // | \| \ + // c---d---e + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), + c = ga.addNode(), d = ga.addNode(), + e = ga.addNode(); + + ga.addEdge(a,b); + ga.addEdge(b,d); + ga.addEdge(d,c); + ga.addEdge(c,a); + ga.addEdge(a,d); + ga.addEdge(b,e); + ga.addEdge(d,e); + + std::vector< Triplet > vec_triplets; + EXPECT_TRUE(List_Triplets(ga, vec_triplets)); + EXPECT_EQ(3, vec_triplets.size()); + } +} + +TEST(TripletFinder, test_for_triplet) { + + typedef lemon::ListGraph Graph; + { + // + // a__b + // |\/| + // |/\| + // c--d + Graph ga; + Graph::Node a = ga.addNode(), b = ga.addNode(), + c = ga.addNode(), d = ga.addNode(); + + ga.addEdge(a,b); + ga.addEdge(a,c); + ga.addEdge(a,d); + ga.addEdge(c,d); + ga.addEdge(b,d); + ga.addEdge(c,b); + + std::vector< Triplet > vec_triplets; + EXPECT_TRUE(List_Triplets(ga, vec_triplets)); + EXPECT_EQ(4, vec_triplets.size()); + } +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From fc676328328712c17afa4a3a6a912a8a672a539f Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 30 Dec 2013 17:21:36 +0100 Subject: [PATCH 11/68] Update the comment. --- src/openMVG/matching/matcher_kdtree_flann.hpp | 4 ++-- src/openMVG/tracks/tracks.hpp | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/openMVG/matching/matcher_kdtree_flann.hpp b/src/openMVG/matching/matcher_kdtree_flann.hpp index 642a852d8b..9c06ade588 100644 --- a/src/openMVG/matching/matcher_kdtree_flann.hpp +++ b/src/openMVG/matching/matcher_kdtree_flann.hpp @@ -95,8 +95,8 @@ class ArrayMatcher_Kdtree_Flann : public ArrayMatcher * * \param[in] query The query array * \param[in] nbQuery The number of query rows - * \param[out] indice The indices of arrays in the dataset that - * have been computed as the nearest arrays. + * \param[out] indice For each "query" it save the index of the "NN" + * nearest entry in the dataset (provided in Build). * \param[out] distance The distances between the matched arrays. * \param[out] NN The number of maximal neighbor that could * will be searched. diff --git a/src/openMVG/tracks/tracks.hpp b/src/openMVG/tracks/tracks.hpp index 1b15450b47..ba24d63875 100644 --- a/src/openMVG/tracks/tracks.hpp +++ b/src/openMVG/tracks/tracks.hpp @@ -197,7 +197,7 @@ struct TracksBuilder } /// Remove the pair that have too few correspondences. - bool FilterPairWiseMinimumMatches(size_t minMatchesOccurences) + bool FilterPairWiseMinimumMatches(size_t minMatchesOccurences, bool bVerbose = false) { std::vector vec_tracksToRemove; std::map< size_t, set > map_tracksIdPerImages; @@ -233,7 +233,8 @@ struct TracksBuilder sort(vec_tracksToRemove.begin(), vec_tracksToRemove.end()); std::vector::iterator it = unique (vec_tracksToRemove.begin(), vec_tracksToRemove.end()); vec_tracksToRemove.resize( std::distance(vec_tracksToRemove.begin(), it) ); - cout << endl << endl < Date: Mon, 30 Dec 2013 17:22:05 +0100 Subject: [PATCH 12/68] Update list of openMVG authors. --- AUTHORS | 1 + 1 file changed, 1 insertion(+) diff --git a/AUTHORS b/AUTHORS index 3059190ce1..519ea39d1e 100644 --- a/AUTHORS +++ b/AUTHORS @@ -7,6 +7,7 @@ Renaud Marlet ySalaun bduisit ORNis +fcastan ------------- OpenMVG authors would thanks the libmv authors From cce66c73a5d22bbe620f4ed6ab36facd5aea9919 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 30 Dec 2013 17:31:38 +0100 Subject: [PATCH 13/68] Minor update for the unit test name. #55 --- src/openMVG/graph/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/openMVG/graph/CMakeLists.txt b/src/openMVG/graph/CMakeLists.txt index 846c9ca6bd..c927cdaa3e 100644 --- a/src/openMVG/graph/CMakeLists.txt +++ b/src/openMVG/graph/CMakeLists.txt @@ -1,5 +1,4 @@ -UNIT_TEST(openMVG connectedComponent "lemon") +UNIT_TEST(openMVG_graph connectedComponent "lemon") UNIT_TEST(openMVG graph_triplet_finder "lemon") - From d51ce63aa566c30d30c9645d14dcca5ac7ad3076 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 30 Dec 2013 17:32:39 +0100 Subject: [PATCH 14/68] Global translations from triplets of relatives heading translations. #54 --- .../lInfinitycomputervision/CMakeLists.txt | 8 +- .../global_translations_fromTriplets.hpp | 209 +++++++++++++++++ .../global_translations_fromTriplets_test.cpp | 211 ++++++++++++++++++ 3 files changed, 426 insertions(+), 2 deletions(-) create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets.hpp create mode 100644 src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets_test.cpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt b/src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt index ea85a2e211..94cc6cebb6 100644 --- a/src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt @@ -29,6 +29,10 @@ UNIT_TEST(openMVG_lInftyComputerVision resection_L_Infty_robust "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") # Translation and Structure -UNIT_TEST(openMVG_lInfinityComputerVision translationAndStructure_L_Infty "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") +UNIT_TEST(openMVG_lInftyComputerVision translationAndStructure_L_Infty "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") # Translation and Structure with noise handling -UNIT_TEST(openMVG_lInfinityComputerVision translationAndStructure_L_Infty_outlier "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") +UNIT_TEST(openMVG_lInftyComputerVision translationAndStructure_L_Infty_outlier "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") + +# Global translations from triplets of heading directions +UNIT_TEST(openMVG_lInftyComputerVision global_translations_fromTriplets "openMVG_multiview_test_data;openMVG_multiview;openMVG_linearProgramming;openMVG_lInftyComputerVision") + diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets.hpp b/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets.hpp new file mode 100644 index 0000000000..e9dd326629 --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets.hpp @@ -0,0 +1,209 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_LINFINITY_COMPUTER_VISION_GLOBAL_TRANSLATIONS_FROMTRIPLETS_H_ +#define OPENMVG_LINFINITY_COMPUTER_VISION_GLOBAL_TRANSLATIONS_FROMTRIPLETS_H_ + +#include "openMVG/numeric/numeric.h" +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include +#include +#include +#include + +//------------------ +//-- Bibliography -- +//------------------ +//- [1] "Global Fusion of Relative Motions for Robust, Accurate and Scalable Structure from Motion." +//- Authors: Pierre MOULON, Pascal MONASSE and Renaud MARLET. +//- Date: December 2013. +//- Conference: ICCV. + +namespace openMVG { +namespace lInfinitycomputervision { + +using namespace linearProgramming; + +typedef std::pair, std::pair > relativeInfo; + +// Setup the linear program to solve the union of trifocal tensors heading +// directions in a common global coordinate system. +//- Implementation of the LINEAR PROGRAM (9) page 5 of [1]: +//-- +static void EncodeTi_from_tij_OneLambdaPerTrif( + const size_t nTranslation, + const std::vector & vec_relative, + sRMat & A, Vec & C, + std::vector & vec_sign, + std::vector & vec_costs, + std::vector< std::pair > & vec_bounds) +{ + // Build Constraint matrix. + + const size_t Ncam = (size_t) nTranslation; + const size_t Nrelative = vec_relative.size(); + + const size_t transStart = 0; + const size_t lambdaStart = 3 * Ncam; + const size_t gammaStart = lambdaStart + Nrelative/3; + +#undef TVAR +#undef LAMBDAVAR +#undef GAMMAVAR +# define TVAR(i, el) (transStart + 3*(i) + (el)) // translation (X,Y,Z) +# define LAMBDAVAR(j) (lambdaStart + (int)((j)/3)) // One per relative translation +# define GAMMAVAR gammaStart + + const size_t Nconstraint = Nrelative * 6; + const size_t NVar = 3 * Ncam + Nrelative/3 + 1; + + A.resize(Nconstraint, NVar); + + C.resize(Nconstraint, 1); + C.fill(0.0); + vec_sign.resize(Nconstraint); + + // By default set free variable: + vec_bounds = std::vector< std::pair >(NVar); + fill( vec_bounds.begin(), vec_bounds.end(), + std::make_pair((double)-1e+30, (double)1e+30)); + // Make first camera at origin (translation ambiguity) + vec_bounds[TVAR(0,0)].first = vec_bounds[TVAR(0,0)].second = 0; + vec_bounds[TVAR(0,1)].first = vec_bounds[TVAR(0,1)].second = 0; + vec_bounds[TVAR(0,2)].first = vec_bounds[TVAR(0,2)].second = 0; + // Make lambda variables between 1 and large number => constraint that lambda_ij > 1 + for (size_t k = 0; k < Nrelative/3; ++k) + vec_bounds[lambdaStart + k].first = 1; + + // Setup gamma >= 0 + vec_bounds[vec_bounds.size()-1].first = 0.0; + + //-- Minimize gamma + vec_costs.resize(NVar); + std::fill(vec_costs.begin(), vec_costs.end(), 0.0); + vec_costs[GAMMAVAR] = 1.0; + //-- + + size_t rowPos = 0; + + for (size_t k = 0; k < Nrelative; ++k) + { + const size_t i = vec_relative[k].first.first; + const size_t j = vec_relative[k].first.second; + const Mat3 & Rij = vec_relative[k].second.first; + const Vec3 & tij = vec_relative[k].second.second; + + // | T_j - R_ij T_i - Lambda_ij t_ij | < Gamma + // Absolute constraint transformed in two sign constraints + // T_j - R_ij T_i - Lambda_ij t_ij < Gamma + // T_j - R_ij T_i - Lambda_ij t_ij > - Gamma + + // For X, Y, Z axis: + for (int l = 0; l < 3; ++l) + { + // T_j + A.coeffRef(rowPos, TVAR(j, l)) = 1; + + //- R_ij T_i + A.coeffRef(rowPos, TVAR(i, 0)) = - Rij(l, 0); + A.coeffRef(rowPos, TVAR(i, 1)) = - Rij(l, 1); + A.coeffRef(rowPos, TVAR(i, 2)) = - Rij(l, 2); + + // - Lambda_ij t_ij + A.coeffRef(rowPos, LAMBDAVAR(k)) = - tij(l); + + // - gamma + A.coeffRef(rowPos, GAMMAVAR) = -1; + + // < 0 + vec_sign[rowPos] = LP_Constraints::LP_LESS_OR_EQUAL; + C(rowPos) = 0; + ++rowPos; + + // --------- + // Opposite constraint + // T_j - R_ij T_i - Lambda_ij t_ij > - Gamma + // --------- + + // T_j + A.coeffRef(rowPos, TVAR(j, l)) = 1; + + //- R_ij T_i + A.coeffRef(rowPos, TVAR(i, 0)) = - Rij(l, 0); + A.coeffRef(rowPos, TVAR(i, 1)) = - Rij(l, 1); + A.coeffRef(rowPos, TVAR(i, 2)) = - Rij(l, 2); + + // - Lambda_ij t_ij + A.coeffRef(rowPos, LAMBDAVAR(k)) = - tij(l); + + // + gamma + A.coeffRef(rowPos, GAMMAVAR) = 1; + + // > 0 + vec_sign[rowPos] = LP_Constraints::LP_GREATER_OR_EQUAL; + C(rowPos) = 0; + ++rowPos; + } + } // end for (k) +#undef TVAR +#undef LAMBDAVAR +#undef GAMMAVAR +} + +//-- Estimate the translation from heading relative translations of triplets. +//- Translation directions must not be normalized (in this way relative scale +//- of relative motions is kept and colinear motion is supported). +struct Tifromtij_ConstraintBuilder_OneLambdaPerTrif +{ + Tifromtij_ConstraintBuilder_OneLambdaPerTrif( + const std::vector< relativeInfo > & vec_relative) + :_vec_relative(vec_relative) + { + //Count the number of camera that are represented + std::set countSet; + for (size_t i = 0; i < vec_relative.size(); ++i) + { + countSet.insert(vec_relative[i].first.first); + countSet.insert(vec_relative[i].first.second); + } + _Ncam = countSet.size(); + } + + /// Setup constraints for the global translations problem, + /// in the LP_Constraints_Sparse object. + bool Build(LP_Constraints_Sparse & constraint) + { + EncodeTi_from_tij_OneLambdaPerTrif( + _Ncam, + _vec_relative, + constraint._constraintMat, + constraint._Cst_objective, + constraint._vec_sign, + constraint._vec_cost, + constraint._vec_bounds); + + // it's a minimization problem over the gamma variable + constraint._bminimize = true; + + //-- Setup additional information about the Linear Program constraint. + // We look for : + // - #translations parameters, + // - #relative lambda factors (one per triplet), + // - one gamma parameter. + constraint._nbParams = _Ncam * 3 + _vec_relative.size()/3 + 1; + return true; + } + + // Internal data + size_t _Ncam; + const std::vector< relativeInfo > & _vec_relative; // /!\ memory Alias +}; + +} // namespace lInfinitycomputervision +} // namespace openMVG + +#endif // OPENMVG_LINFINITY_COMPUTER_VISION_GLOBAL_TRANSLATIONS_FROMTRIPLETS_H_ + diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets_test.cpp b/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets_test.cpp new file mode 100644 index 0000000000..6abe54c4dc --- /dev/null +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets_test.cpp @@ -0,0 +1,211 @@ + +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#include "openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets.hpp" + +#include "openMVG/multiview/essential.hpp" + +#include "openMVG/graph/graph_triplet_finder.hpp" +using namespace openMVG::graphUtils; + +#include "third_party/vectorGraphics/svgDrawer.hpp" +using namespace svg; + +#include "openMVG/multiview/test_data_sets.hpp" +#include "testing/testing.h" + +#include +#include +#include +#include + +using namespace openMVG; +using namespace openMVG::linearProgramming; +using namespace lInfinitycomputervision; +using namespace std; + +int modifiedMod(int number, int modulus) +{ + int result = number % modulus; + if (result < 0) result += modulus; + return result; +} + +//-- Export a series of camera positions to a SVG surface of specified squared size +void visibleCamPosToSVGSurface( + const std::vector & vec_Ci, + const std::string & fileName) +{ + Mat points(3, vec_Ci.size()); + for(size_t i = 0; i < vec_Ci.size(); ++i) + { + points.col(i) = vec_Ci[i]; + } + + Vec mean, variance; + MeanAndVarianceAlongRows(points, &mean, &variance); + + double xfactor = sqrt(2.0 / variance(0)); + double yfactor = sqrt(2.0 / variance(2)); + + std::vector out = vec_Ci; + for(size_t i = 0; i < vec_Ci.size(); ++i) + { + out[i](0) = ((out[i](0) * xfactor) + -xfactor * mean(0)) * 30 + 100; + out[i](2) = ((out[i](2) * yfactor) + -yfactor * mean(2)) * 30 + 100; + } + + double size = 200; + if (!fileName.empty()) + { + svgDrawer svgSurface_GT(size,size); + for(size_t i = 0; i < vec_Ci.size(); ++i) + { + svgSurface_GT.drawCircle(out[i](0), out[i](2), + 3,svgStyle().stroke("black",0.2).fill("red")); + } + std::ostringstream osSvgGT; + osSvgGT << fileName; + std::ofstream svgFileGT( osSvgGT.str().c_str()); + svgFileGT << svgSurface_GT.closeSvgFile().str(); + } +} + +TEST(translation_averaging, globalTi_from_tijs_Triplets) { + + int focal = 1000; + int principal_Point = 500; + //-- Setup a circular camera rig or "cardiod". + const int iNviews = 12; + const int iNbPoints = 6; + NViewDataSet d = + //NRealisticCamerasRing( + NRealisticCamerasCardioid( + iNviews, iNbPoints, + nViewDatasetConfigurator(focal,focal,principal_Point,principal_Point,5,0)); + + d.ExportToPLY("global_translations_from_triplets_GT.ply"); + + visibleCamPosToSVGSurface(d._C, "global_translations_from_triplets_GT.svg"); + + // List sucessives triplets of the large loop of camera + std::vector< graphUtils::Triplet > vec_triplets; + for (int i = 0; i < iNviews; ++i) + { + const int iPlus1 = modifiedMod(i+1,iNviews); + const int iPlus2 = modifiedMod(i+2,iNviews); + //-- sort the triplet index to have a monotic ascending series of value + double triplet[3] = {i, iPlus1, iPlus2}; + std::sort(&triplet[0], &triplet[3]); + vec_triplets.push_back(Triplet(triplet[0],triplet[1],triplet[2])); + } + + //- For each triplet compute relative translations and rotations motions + std::vector vec_initialEstimates; + + for (size_t i = 0; i < vec_triplets.size(); ++i) + { + const graphUtils::Triplet & triplet = vec_triplets[i]; + size_t I = triplet.i, J = triplet.j , K = triplet.k; + + //-- Build camera alias over GT translations and rotations: + const Mat3 & RI = d._R[I]; + const Mat3 & RJ = d._R[J]; + const Mat3 & RK = d._R[K]; + const Vec3 & ti = d._t[I]; + const Vec3 & tj = d._t[J]; + const Vec3 & tk = d._t[K]; + + //-- Build relatives motions (that feeds the Linear program formulation) + { + Mat3 RijGt; + Vec3 tij; + RelativeCameraMotion(RI, ti, RJ, tj, &RijGt, &tij); + vec_initialEstimates.push_back( + std::make_pair(std::make_pair(I, J), std::make_pair(RijGt, tij))); + + Mat3 RjkGt; + Vec3 tjk; + RelativeCameraMotion(RJ, tj, RK, tk, &RjkGt, &tjk); + vec_initialEstimates.push_back( + std::make_pair(std::make_pair(J, K), std::make_pair(RjkGt, tjk))); + + Mat3 RikGt; + Vec3 tik; + RelativeCameraMotion(RI, ti, RK, tk, &RikGt, &tik); + vec_initialEstimates.push_back( + std::make_pair(std::make_pair(I, K), std::make_pair(RikGt, tik))); + } + } + + //-- Compute the global translations from the triplets of heading directions + //- with the L_infinity optimization + + std::vector vec_solution(iNviews*3 + vec_initialEstimates.size()/3 + 1); + double gamma = -1.0; + + //- a. Setup the LP solver, + //- b. Setup the constraints generator (for the dedicated L_inf problem), + //- c. Build constraints and solve the problem, + //- d. Get back the estimated parameters. + + //- a. Setup the LP solver, + OSI_CLP_SolverWrapper solverLP(vec_solution.size()); + + //- b. Setup the constraints generator (for the dedicated L_inf problem), + Tifromtij_ConstraintBuilder_OneLambdaPerTrif cstBuilder(vec_initialEstimates); + + //- c. Build constraints and solve the problem (Setup constraints and solver) + LP_Constraints_Sparse constraint; + cstBuilder.Build(constraint); + solverLP.setup(constraint); + //-- Solving + EXPECT_TRUE(solverLP.solve()); // the linear program must have a solution + + //- d. Get back the estimated parameters. + solverLP.getSolution(vec_solution); + gamma = vec_solution[vec_solution.size()-1]; + + //-- + //-- Unit test checking about the found solution + //-- + EXPECT_NEAR(0.0, gamma, 1e-6); // Gamma must be 0, no noise, perfect data have been sent + + std::cout << "Found solution with gamma = " << gamma << std::endl; + + //-- Get back computed camera translations + std::vector vec_camTranslation(iNviews*3,0); + std::copy(&vec_solution[0], &vec_solution[iNviews*3], &vec_camTranslation[0]); + + //-- Get back computed lambda factors + std::vector vec_camRelLambdas(&vec_solution[iNviews*3], &vec_solution[iNviews*3 + vec_initialEstimates.size()/3]); + // lambda factors must be equal to 1.0 (no compression, no dilation); + EXPECT_NEAR(vec_initialEstimates.size()/3, std::accumulate (vec_camRelLambdas.begin(), vec_camRelLambdas.end(), 0.0), 1e-6); + + // True camera C + std::cout << std::endl << "Camera centers (Ground truth): " << std::endl; + for (size_t i = 0; i < iNviews; ++i) + { + std::cout << i << ": " << d._C[i].transpose() - d._C[0].transpose() << std::endl; + } + + // Get back the camera translations in the global frame: + std::cout << std::endl << "Camera centers (Computed): " << std::endl; + for (size_t i = 0; i < iNviews; ++i) + { + const Vec3 C_GT = d._C[i].transpose() - d._C[0].transpose(); //First camera supposed to be at Identity + + Vec3 t(vec_camTranslation[i*3], vec_camTranslation[i*3+1], vec_camTranslation[i*3+2]); + const Mat3 & Ri = d._R[i]; + const Vec3 C_computed = - Ri.transpose() * t; + + //-- Check that found camera position is equal to GT value + EXPECT_NEAR(0.0, DistanceLInfinity(C_computed, C_GT), 1e-6); + + std::cout << i << ": " << C_computed.transpose() << std::endl; + } +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From b9e7a3bc1b77ed6ada55464108687bdbfdfe9329 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 30 Dec 2013 17:45:31 +0100 Subject: [PATCH 15/68] Add a 7dof rigid registration between point clouds (sRT). #56 --- src/openMVG/CMakeLists.txt | 14 +- src/openMVG/geometry/CMakeLists.txt | 3 + .../geometry/rigid_transformation3D_srt.hpp | 241 ++++++++++++++++++ .../rigid_transformation3D_srt_test.cpp | 119 +++++++++ 4 files changed, 370 insertions(+), 7 deletions(-) create mode 100644 src/openMVG/geometry/CMakeLists.txt create mode 100644 src/openMVG/geometry/rigid_transformation3D_srt.hpp create mode 100644 src/openMVG/geometry/rigid_transformation3D_srt_test.cpp diff --git a/src/openMVG/CMakeLists.txt b/src/openMVG/CMakeLists.txt index 8e04220cdc..29a9308729 100644 --- a/src/openMVG/CMakeLists.txt +++ b/src/openMVG/CMakeLists.txt @@ -1,14 +1,14 @@ -ADD_SUBDIRECTORY(image) +ADD_SUBDIRECTORY(bundle_adjustment) +ADD_SUBDIRECTORY(exif_IO) ADD_SUBDIRECTORY(features) +ADD_SUBDIRECTORY(geometry) +ADD_SUBDIRECTORY(graph) +ADD_SUBDIRECTORY(image) +ADD_SUBDIRECTORY(linearProgramming) ADD_SUBDIRECTORY(matching) ADD_SUBDIRECTORY(multiview) ADD_SUBDIRECTORY(numeric) -ADD_SUBDIRECTORY(tracks) -ADD_SUBDIRECTORY(graph) ADD_SUBDIRECTORY(robust_estimation) -ADD_SUBDIRECTORY(bundle_adjustment) - -ADD_SUBDIRECTORY(exif_IO) ADD_SUBDIRECTORY(split) +ADD_SUBDIRECTORY(tracks) -ADD_SUBDIRECTORY(linearProgramming) diff --git a/src/openMVG/geometry/CMakeLists.txt b/src/openMVG/geometry/CMakeLists.txt new file mode 100644 index 0000000000..0a3af841e2 --- /dev/null +++ b/src/openMVG/geometry/CMakeLists.txt @@ -0,0 +1,3 @@ + +UNIT_TEST(openMVG rigid_transformation3D_srt "openMVG_numeric") + diff --git a/src/openMVG/geometry/rigid_transformation3D_srt.hpp b/src/openMVG/geometry/rigid_transformation3D_srt.hpp new file mode 100644 index 0000000000..a59c9a91c7 --- /dev/null +++ b/src/openMVG/geometry/rigid_transformation3D_srt.hpp @@ -0,0 +1,241 @@ +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_GEOMETRY_3D_REGISTRATION_7DOF_H_ +#define OPENMVG_GEOMETRY_3D_REGISTRATION_7DOF_H_ + +#include "openMVG/numeric/numeric.h" +#include "openMVG/numeric/lm.hpp" + +namespace openMVG { +namespace geometry { + +/** 3D rigid transformation estimation (7 dof) + * Compute a Scale Rotation and Translation rigid transformation. + * This transformation provide a distortion-free transformation + * using the following formulation Xb = S * R * Xa + t. + * Haralick, Robert, Shapiro, Linda. Computer and Robot Vision book, 1992. + * + * \param[in] x1 The first 3xN matrix of euclidean points + * \param[in] x2 The second 3xN matrix of euclidean points + * \param[out] S The scale factor + * \param[out] t The 3x1 translation + * \param[out] R The 3x3 rotation + * + * \return true if the transformation estimation has succeeded + * + * \note Need at least 3 points + */ + +static bool FindRTS(const Mat &x1, + const Mat &x2, + double * S, + Vec3 * t, + Mat3 * R) +{ + assert(3 == x1.rows()); + assert(3 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + const int n = static_cast(x1.cols()); + + // Compute scale factor + double S1 =0.0, S2=0.0; + S1 = (x1.array()*x1.array()).sum(); + S2 = (x2.array()*x2.array()).sum(); + + *S = sqrt(S2/S1); + + // Compute centroid and variance + Vec meanx1, meanx2; + Vec varx1, varx2; + MeanAndVarianceAlongRows(x1, &meanx1, &varx1); + MeanAndVarianceAlongRows(x2, &meanx2, &varx2); + + // Compute the n*n correlation matrix + Mat p1c(3,n) , p2c(3,n); + + for(int i = 0; i < n; ++i){ + p1c.col(i) = x1.col(i) - meanx1; + p2c.col(i) = x2.col(i) - meanx2; + } + + Mat H = p1c * p2c.transpose(); + + Eigen::JacobiSVD svdObj(H,Eigen::ComputeFullV | Eigen::ComputeFullU); + Mat V = svdObj.matrixV(); + Mat U = svdObj.matrixU(); + *R = V * U.transpose(); + + if( R->determinant() < 0){ + return false; + } + + //Compute the optimal translation + *t = meanx2 - (*S) * (*R) * meanx1; + + return true; +} + +// Eigen LM functor to refine translation, Rotation and Scale parameter. +struct lm_SRTRefine_functor : Functor +{ + lm_SRTRefine_functor(int inputs, int values, + const Mat &x1, const Mat &x2, + const double &S, const Mat3 & R, const Vec &t): Functor(inputs,values), + _x1(x1), _x2(x2), _t(t), _R(R), _S(S) { } + + int operator()(const Vec &x, Vec &fvec) const + { + // convert x to rotation matrix and a translation vector and a Scale factor + // x = {tx,ty,tz,anglex,angley,anglez,S} + Vec3 transAdd = x.block<3,1>(0,0); + Vec3 rot = x.block<3,1>(3,0); + double Sadd = x(6); + + //Build the rotation matrix + Mat3 Rcor = + (Eigen::AngleAxis(rot(0), Vec3::UnitX()) + * Eigen::AngleAxis(rot(1), Vec3::UnitY()) + * Eigen::AngleAxis(rot(2), Vec3::UnitZ())).toRotationMatrix(); + + const Mat3 nR = _R*Rcor; + const Vec3 nt = _t+transAdd; + const double nS = _S+Sadd; + + // Evaluate re-projection errors + Vec3 proj; + for (Mat::Index i = 0; i < _x1.cols(); ++i) + { + proj = _x2.col(i) - (nS * nR * (_x1.col(i)) + nt); + fvec[i*3] = proj(0); + fvec[i*3+1] = proj(1); + fvec[i*3+2] = proj(2); + } + return 0; + } + + Mat _x1, _x2; + Vec3 _t; + Mat3 _R; + double _S; +}; + +// Eigen LM functor to refine Rotation. +struct lm_RRefine_functor : Functor +{ + lm_RRefine_functor(int inputs, int values, + const Mat &x1, const Mat &x2, + const double &S, const Mat3 & R, const Vec &t): Functor(inputs,values), + _x1(x1), _x2(x2), _t(t), _R(R), _S(S) { } + + int operator()(const Vec &x, Vec &fvec) const + { + // convert x to rotation matrix + // x = {anglex,angley,anglez} + Vec3 rot = x.block<3,1>(0,0); + + //Build the rotation matrix + Mat3 Rcor = + (Eigen::AngleAxis(rot(0), Vec3::UnitX()) + * Eigen::AngleAxis(rot(1), Vec3::UnitY()) + * Eigen::AngleAxis(rot(2), Vec3::UnitZ())).toRotationMatrix(); + + const Mat3 nR = _R*Rcor; + const Vec3 nt = _t; + const double nS = _S; + + // Evaluate re-projection errors + Vec3 proj; + for (Mat::Index i = 0; i < _x1.cols(); ++i) + { + proj = _x2.col(i) - (nS * nR * (_x1.col(i)) + nt); + fvec[i*3] = proj(0); + fvec[i*3+1] = proj(1); + fvec[i*3+2] = proj(2); + } + return 0; + } + + Mat _x1, _x2; + Vec3 _t; + Mat3 _R; + double _S; +}; + +/** 3D rigid transformation refinement using LM + * Refine the Scale, Rotation and translation rigid transformation + * using a Levenberg-Marquardt opimization. + * + * \param[in] x1 The first 3xN matrix of euclidean points + * \param[in] x2 The second 3xN matrix of euclidean points + * \param[out] S The initial scale factor + * \param[out] t The initial 3x1 translation + * \param[out] R The initial 3x3 rotation + * + * \return none + */ +static void Refine_RTS(const Mat &x1, + const Mat &x2, + double * S, + Vec3 * t, + Mat3 * R) +{ + { + lm_SRTRefine_functor functor(7, 3*x1.cols(), x1, x2, *S, *R, *t); + + Eigen::NumericalDiff numDiff(functor); + + Eigen::LevenbergMarquardt > lm(numDiff); + lm.parameters.maxfev = 1000; + Vec xlm = Vec::Zero(7); // The deviation vector {tx,ty,tz,rotX,rotY,rotZ,S} + + int info = lm.minimize(xlm); + + Vec3 transAdd = xlm.block<3,1>(0,0); + Vec3 rot = xlm.block<3,1>(3,0); + double SAdd = xlm(6); + + //Build the rotation matrix + Mat3 Rcor = + (Eigen::AngleAxis(rot(0), Vec3::UnitX()) + * Eigen::AngleAxis(rot(1), Vec3::UnitY()) + * Eigen::AngleAxis(rot(2), Vec3::UnitZ())).toRotationMatrix(); + + *R = (*R)*Rcor; + *t = (*t)+transAdd; + *S = (*S)+SAdd; + } + + // Refine rotation + { + lm_RRefine_functor functor(3, 3*x1.cols(), x1, x2, *S, *R, *t); + + Eigen::NumericalDiff numDiff(functor); + + Eigen::LevenbergMarquardt > lm(numDiff); + lm.parameters.maxfev = 1000; + Vec xlm = Vec::Zero(3); // The deviation vector {rotX,rotY,rotZ} + + int info = lm.minimize(xlm); + + Vec3 rot = xlm.block<3,1>(0,0); + + //Build the rotation matrix + Mat3 Rcor = + (Eigen::AngleAxis(rot(0), Vec3::UnitX()) + * Eigen::AngleAxis(rot(1), Vec3::UnitY()) + * Eigen::AngleAxis(rot(2), Vec3::UnitZ())).toRotationMatrix(); + + *R = (*R)*Rcor; + } +} + +} // namespace geometry +} // namespace openMVG + +#endif // OPENMVG_GEOMETRY_REGISTRATION_7DOF_H_ diff --git a/src/openMVG/geometry/rigid_transformation3D_srt_test.cpp b/src/openMVG/geometry/rigid_transformation3D_srt_test.cpp new file mode 100644 index 0000000000..9d2e4bbc45 --- /dev/null +++ b/src/openMVG/geometry/rigid_transformation3D_srt_test.cpp @@ -0,0 +1,119 @@ +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "openMVG/geometry/rigid_transformation3D_srt.hpp" +#include "CppUnitLite/TestHarness.h" +#include "testing/testing.h" +#include + +using namespace openMVG; +using namespace openMVG::geometry; +using namespace std; + +TEST(SRT_precision, Experiment_ScaleOnly) { + + int nbPoints = 10; + Mat x1 = Mat::Random(3,nbPoints); + Mat x2 = x1; + + double scale = 2; + Mat3 rot = Mat3::Identity(); + Vec3 t(0,0,0); + + for (int i=0; i < nbPoints; ++i) + { + Vec3 pt = x1.col(i); + x2.col(i) = (scale * rot * pt + t); + } + + // Compute the Similarity transform + double Sc; + Mat3 Rc; + Vec3 tc; + FindRTS(x1, x2, &Sc, &tc, &Rc); + Refine_RTS(x1,x2,&Sc,&tc,&Rc); + + std::cout << "\n" + << "Scale " << Sc << "\n" + << "Rot \n" << Rc << "\n" + << "t " << tc.transpose(); +} + +TEST(SRT_precision, Experiment_ScaleAndRot) { + + int nbPoints = 10; + Mat x1 = Mat::Random(3,nbPoints); + Mat x2 = x1; + + double scale = 2; + Mat3 rot = (Eigen::AngleAxis(.2, Vec3::UnitX()) + * Eigen::AngleAxis(.3, Vec3::UnitY()) + * Eigen::AngleAxis(.6, Vec3::UnitZ())).toRotationMatrix(); + Vec3 t(0,0,0); + + for (int i=0; i < nbPoints; ++i) + { + Vec3 pt = x1.col(i); + x2.col(i) = (scale * rot * pt + t); + } + + // Compute the Similarity transform + double Sc; + Mat3 Rc; + Vec3 tc; + FindRTS(x1, x2, &Sc, &tc, &Rc); + Refine_RTS(x1,x2,&Sc,&tc,&Rc); + + std::cout << "\n" + << "Scale " << Sc << "\n" + << "Rot \n" << Rc << "\n" + << "t " << tc.transpose(); + + std::cout << "\nGT\n" + << "Scale " << scale << "\n" + << "Rot \n" << rot << "\n" + << "t " << t.transpose(); +} + +TEST(SRT_precision, Experiment_ScaleRotTranslation) { + + int nbPoints = 10; + Mat x1 = Mat::Random(3,nbPoints); + Mat x2 = x1; + + double scale = 2; + Mat3 rot = (Eigen::AngleAxis(.2, Vec3::UnitX()) + * Eigen::AngleAxis(.3, Vec3::UnitY()) + * Eigen::AngleAxis(.6, Vec3::UnitZ())).toRotationMatrix(); + Vec3 t(0.5,-0.3,.38); + + for (int i=0; i < nbPoints; ++i) + { + Vec3 pt = x1.col(i); + x2.col(i) = (scale * rot * pt + t); + } + + // Compute the Similarity transform + double Sc; + Mat3 Rc; + Vec3 tc; + FindRTS(x1, x2, &Sc, &tc, &Rc); + Refine_RTS(x1,x2,&Sc,&tc,&Rc); + + std::cout << "\n" + << "Scale " << Sc << "\n" + << "Rot \n" << Rc << "\n" + << "t " << tc.transpose(); + + std::cout << "\nGT\n" + << "Scale " << scale << "\n" + << "Rot \n" << rot << "\n" + << "t " << t.transpose(); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ From 83a7025c799b035a83d0738a88d2dcab5dd1dc5b Mon Sep 17 00:00:00 2001 From: pmoulon Date: Tue, 31 Dec 2013 10:13:24 +0100 Subject: [PATCH 16/68] rename graph_triplet_finder.hpp to triplet_finder.hpp. #55 --- src/openMVG/graph/CMakeLists.txt | 2 +- .../{graph_triplet_finder.hpp => triplet_finder.hpp} | 2 +- ...triplet_finder_test.cpp => triplet_finder_test.cpp} | 2 +- .../global_translations_fromTriplets_test.cpp | 10 +++++----- 4 files changed, 8 insertions(+), 8 deletions(-) rename src/openMVG/graph/{graph_triplet_finder.hpp => triplet_finder.hpp} (99%) rename src/openMVG/graph/{graph_triplet_finder_test.cpp => triplet_finder_test.cpp} (99%) diff --git a/src/openMVG/graph/CMakeLists.txt b/src/openMVG/graph/CMakeLists.txt index c927cdaa3e..aa89b90b39 100644 --- a/src/openMVG/graph/CMakeLists.txt +++ b/src/openMVG/graph/CMakeLists.txt @@ -1,4 +1,4 @@ UNIT_TEST(openMVG_graph connectedComponent "lemon") -UNIT_TEST(openMVG graph_triplet_finder "lemon") +UNIT_TEST(openMVG_graph triplet_finder "lemon") diff --git a/src/openMVG/graph/graph_triplet_finder.hpp b/src/openMVG/graph/triplet_finder.hpp similarity index 99% rename from src/openMVG/graph/graph_triplet_finder.hpp rename to src/openMVG/graph/triplet_finder.hpp index a7e9bbd37c..a7b131c3f1 100644 --- a/src/openMVG/graph/graph_triplet_finder.hpp +++ b/src/openMVG/graph/triplet_finder.hpp @@ -94,7 +94,7 @@ bool List_Triplets(const GraphT & g, std::vector< Triplet > & vec_triplets) if (cycleEdge!= INVALID && !map_edge[cycleEdge]) { // Elementary cycle found (make value follow a monotonic ascending serie) - double triplet[3] = { + int triplet[3] = { g.id(itNode), g.id(g.target(itPrev)), g.id(g.target(vec_edges[i]))}; diff --git a/src/openMVG/graph/graph_triplet_finder_test.cpp b/src/openMVG/graph/triplet_finder_test.cpp similarity index 99% rename from src/openMVG/graph/graph_triplet_finder_test.cpp rename to src/openMVG/graph/triplet_finder_test.cpp index 246d1cfed7..483864045d 100644 --- a/src/openMVG/graph/graph_triplet_finder_test.cpp +++ b/src/openMVG/graph/triplet_finder_test.cpp @@ -4,7 +4,7 @@ // License, v. 2.0. If a copy of the MPL was not distributed with this // file, You can obtain one at http://mozilla.org/MPL/2.0/. -#include "openMVG/graph/graph_triplet_finder.hpp" +#include "openMVG/graph/triplet_finder.hpp" using namespace openMVG::graphUtils; #include "CppUnitLite/TestHarness.h" diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets_test.cpp b/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets_test.cpp index 6abe54c4dc..5ea748a9d6 100644 --- a/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets_test.cpp +++ b/src/openMVG/linearProgramming/lInfinitycomputervision/global_translations_fromTriplets_test.cpp @@ -5,7 +5,7 @@ #include "openMVG/multiview/essential.hpp" -#include "openMVG/graph/graph_triplet_finder.hpp" +#include "openMVG/graph/triplet_finder.hpp" using namespace openMVG::graphUtils; #include "third_party/vectorGraphics/svgDrawer.hpp" @@ -90,12 +90,12 @@ TEST(translation_averaging, globalTi_from_tijs_Triplets) { // List sucessives triplets of the large loop of camera std::vector< graphUtils::Triplet > vec_triplets; - for (int i = 0; i < iNviews; ++i) + for (size_t i = 0; i < iNviews; ++i) { - const int iPlus1 = modifiedMod(i+1,iNviews); - const int iPlus2 = modifiedMod(i+2,iNviews); + const size_t iPlus1 = modifiedMod(i+1,iNviews); + const size_t iPlus2 = modifiedMod(i+2,iNviews); //-- sort the triplet index to have a monotic ascending series of value - double triplet[3] = {i, iPlus1, iPlus2}; + size_t triplet[3] = {i, iPlus1, iPlus2}; std::sort(&triplet[0], &triplet[3]); vec_triplets.push_back(Triplet(triplet[0],triplet[1],triplet[2])); } From ddbd77f327187a1cee54f1b7a6bf799f0a5a210e Mon Sep 17 00:00:00 2001 From: pmoulon Date: Tue, 31 Dec 2013 10:13:57 +0100 Subject: [PATCH 17/68] Add openMVG licence. --- src/software/SfM/ImageCollectionGeometricFilter.hpp | 7 +++++++ src/software/SfM/ImageCollectionMatcher.hpp | 7 +++++++ src/software/SfM/ImageCollectionMatcher_AllInMemory.hpp | 8 ++++++++ src/software/SfM/ImageCollection_F_ACRobust.hpp | 8 +++++++- src/software/SfM/SfMRobust.hpp | 7 ++----- src/software/SfM/main_computeMatches.cpp | 9 +-------- 6 files changed, 32 insertions(+), 14 deletions(-) diff --git a/src/software/SfM/ImageCollectionGeometricFilter.hpp b/src/software/SfM/ImageCollectionGeometricFilter.hpp index d61b3476ad..33472ec5cd 100644 --- a/src/software/SfM/ImageCollectionGeometricFilter.hpp +++ b/src/software/SfM/ImageCollectionGeometricFilter.hpp @@ -1,3 +1,10 @@ + +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + #pragma once #include "openMVG/features/features.hpp" diff --git a/src/software/SfM/ImageCollectionMatcher.hpp b/src/software/SfM/ImageCollectionMatcher.hpp index 9850ade987..0d034b44fc 100644 --- a/src/software/SfM/ImageCollectionMatcher.hpp +++ b/src/software/SfM/ImageCollectionMatcher.hpp @@ -1,3 +1,10 @@ + +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + #pragma once #include "openMVG/matching/indMatch.hpp" diff --git a/src/software/SfM/ImageCollectionMatcher_AllInMemory.hpp b/src/software/SfM/ImageCollectionMatcher_AllInMemory.hpp index fe7358cc91..bb7cb6513a 100644 --- a/src/software/SfM/ImageCollectionMatcher_AllInMemory.hpp +++ b/src/software/SfM/ImageCollectionMatcher_AllInMemory.hpp @@ -1,3 +1,11 @@ + +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + + #pragma once #include "software/SfM/ImageCollectionMatcher.hpp" diff --git a/src/software/SfM/ImageCollection_F_ACRobust.hpp b/src/software/SfM/ImageCollection_F_ACRobust.hpp index 944800f2ba..5f07e58903 100644 --- a/src/software/SfM/ImageCollection_F_ACRobust.hpp +++ b/src/software/SfM/ImageCollection_F_ACRobust.hpp @@ -1,5 +1,11 @@ -#pragma once +// Copyright (c) 2012, 2013 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#pragma once #include "openMVG/multiview/solver_fundamental_kernel.hpp" #include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" diff --git a/src/software/SfM/SfMRobust.hpp b/src/software/SfM/SfMRobust.hpp index 63a8ec9611..f44504887e 100644 --- a/src/software/SfM/SfMRobust.hpp +++ b/src/software/SfM/SfMRobust.hpp @@ -59,7 +59,7 @@ bool robustEssential(const Mat3 & K1, const Mat3 & K2, // Robustly estimation of the Essential matrix and it's precision std::pair ACRansacOut = ACRANSAC(kernel, *pvec_inliers, - ACRANSAC_ITER, pE, precision, true); + ACRANSAC_ITER, pE, precision, false); *errorMax = ACRansacOut.first; return pvec_inliers->size() > 2.5 * SolverType::MINIMUM_SAMPLES; @@ -110,10 +110,7 @@ bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2, } } } - // Check the solution : - std::cout << std::endl << "bundlerHelp::estimate_Rt_fromE" << std::endl; - std::cout << "\t Number of points in front of both cameras:" - << f[0] << " " << f[1] << " " << f[2] << " " << f[3] << std::endl; + // Check the solution: std::vector::iterator iter = max_element(f.begin(), f.end()); if(*iter != 0) { size_t index = std::distance(f.begin(),iter); diff --git a/src/software/SfM/main_computeMatches.cpp b/src/software/SfM/main_computeMatches.cpp index fec8ca9007..b18971dc3c 100644 --- a/src/software/SfM/main_computeMatches.cpp +++ b/src/software/SfM/main_computeMatches.cpp @@ -19,12 +19,6 @@ #include "openMVG/matching/matcher_kdtree_flann.hpp" #include "openMVG/matching/indMatch_utils.hpp" -/// Generic Image Collection image matching -#include "software/SfM/ImageCollectionMatcher_AllInMemory.hpp" -#include "software/SfM/ImageCollectionGeometricFilter.hpp" -#include "software/SfM/ImageCollection_F_ACRobust.hpp" -#include "software/SfM/pairwiseAdjacencyDisplay.hpp" - /// Feature detector and descriptor interface #include "patented/sift/SIFT.hpp" @@ -52,7 +46,6 @@ int main(int argc, char **argv) std::string sOutDir = ""; float fDistRatio = .6f; bool bOctMinus1 = false; - size_t coefZoom = 1; float dPeakThreshold = 0.04f; cmd.add( make_option('i', sImaDirectory, "imadir") ); @@ -164,7 +157,7 @@ int main(int argc, char **argv) std::string sDesc = stlplus::create_filespec(sOutDir, stlplus::basename_part(vec_fileNames[i]), "desc"); - //Test if descriptor and feature was already computed + //Test if descriptors and features was already computed if (stlplus::file_exists(sFeat) && stlplus::file_exists(sDesc)) { if (ReadImage(vec_fileNames[i].c_str(), &imageRGB)) { From 0280b2636f8920bfdcaed4548cf452198b305627 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Fri, 3 Jan 2014 12:26:48 +0100 Subject: [PATCH 18/68] Fix compilation --- src/openMVG/matching/kvld/kvld.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/openMVG/matching/kvld/kvld.cpp b/src/openMVG/matching/kvld/kvld.cpp index fd9567d89c..a813b0b6d1 100644 --- a/src/openMVG/matching/kvld/kvld.cpp +++ b/src/openMVG/matching/kvld/kvld.cpp @@ -508,6 +508,3 @@ void writeResult( const string output, } kvldScore.close(); } - -} - From 94fd455e44ad51828cd63cc83d924d4ca2e6a14f Mon Sep 17 00:00:00 2001 From: pmoulon Date: Fri, 3 Jan 2014 13:59:44 +0100 Subject: [PATCH 19/68] Rename lInfinitycomputervision in lInfinityCV --- src/openMVG/linearProgramming/CMakeLists.txt | 2 +- .../{lInfinitycomputervision => lInfinityCV}/CMakeLists.txt | 0 .../{lInfinitycomputervision => lInfinityCV}/resection.hpp | 0 .../resection_L_Infty_robust_test.cpp | 0 .../resection_L_Infty_test.cpp | 0 .../resection_kernel.cpp | 0 .../resection_kernel.hpp | 0 .../translationAndStructureFrom_xi_Ri.hpp | 0 .../translationAndStructureFrom_xi_Ri_noise.hpp | 0 .../translationAndStructure_L_Infty_outlier_test.cpp | 0 .../translationAndStructure_L_Infty_test.cpp | 0 .../{lInfinitycomputervision => lInfinityCV}/triangulation.hpp | 0 .../triangulation_L_Infty_test.cpp | 0 13 files changed, 1 insertion(+), 1 deletion(-) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/CMakeLists.txt (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/resection.hpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/resection_L_Infty_robust_test.cpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/resection_L_Infty_test.cpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/resection_kernel.cpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/resection_kernel.hpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/translationAndStructureFrom_xi_Ri.hpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/translationAndStructureFrom_xi_Ri_noise.hpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/translationAndStructure_L_Infty_outlier_test.cpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/translationAndStructure_L_Infty_test.cpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/triangulation.hpp (100%) rename src/openMVG/linearProgramming/{lInfinitycomputervision => lInfinityCV}/triangulation_L_Infty_test.cpp (100%) diff --git a/src/openMVG/linearProgramming/CMakeLists.txt b/src/openMVG/linearProgramming/CMakeLists.txt index f84aff85b1..f306811917 100644 --- a/src/openMVG/linearProgramming/CMakeLists.txt +++ b/src/openMVG/linearProgramming/CMakeLists.txt @@ -32,4 +32,4 @@ ENDIF (MOSEK_FOUND) UNIT_TEST(openMVG_linearProgramming linearProgramming "openMVG_linearProgramming") -ADD_SUBDIRECTORY(lInfinitycomputervision) +ADD_SUBDIRECTORY(lInfinityCV) diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt b/src/openMVG/linearProgramming/lInfinityCV/CMakeLists.txt similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/CMakeLists.txt rename to src/openMVG/linearProgramming/lInfinityCV/CMakeLists.txt diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection.hpp b/src/openMVG/linearProgramming/lInfinityCV/resection.hpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/resection.hpp rename to src/openMVG/linearProgramming/lInfinityCV/resection.hpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_robust_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_robust_test.cpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_robust_test.cpp rename to src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_robust_test.cpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_test.cpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/resection_L_Infty_test.cpp rename to src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_test.cpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.cpp b/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.cpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.cpp rename to src/openMVG/linearProgramming/lInfinityCV/resection_kernel.cpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp b/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.hpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp rename to src/openMVG/linearProgramming/lInfinityCV/resection_kernel.hpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri.hpp b/src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri.hpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri.hpp rename to src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri.hpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri_noise.hpp b/src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri_noise.hpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri_noise.hpp rename to src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri_noise.hpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_outlier_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_outlier_test.cpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_outlier_test.cpp rename to src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_outlier_test.cpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_test.cpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/translationAndStructure_L_Infty_test.cpp rename to src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_test.cpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/triangulation.hpp b/src/openMVG/linearProgramming/lInfinityCV/triangulation.hpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/triangulation.hpp rename to src/openMVG/linearProgramming/lInfinityCV/triangulation.hpp diff --git a/src/openMVG/linearProgramming/lInfinitycomputervision/triangulation_L_Infty_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/triangulation_L_Infty_test.cpp similarity index 100% rename from src/openMVG/linearProgramming/lInfinitycomputervision/triangulation_L_Infty_test.cpp rename to src/openMVG/linearProgramming/lInfinityCV/triangulation_L_Infty_test.cpp From 1d73c9a1da0491e9ac5cbfe1c0c8ed23540acba1 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Fri, 3 Jan 2014 14:05:24 +0100 Subject: [PATCH 20/68] Make lInfinityCV module filename more concise. --- .../linearProgramming/lInfinityCV/CMakeLists.txt | 10 +++++----- .../linearProgramming/lInfinityCV/resection.hpp | 4 ++-- .../linearProgramming/lInfinityCV/resection_kernel.cpp | 10 +++++----- .../linearProgramming/lInfinityCV/resection_kernel.hpp | 4 ++-- ...Infty_robust_test.cpp => resection_robust_test.cpp} | 6 +++--- .../{resection_L_Infty_test.cpp => resection_test.cpp} | 6 +++--- ...ructureFrom_xi_Ri.hpp => tijsAndXis_From_xi_Ri.hpp} | 6 +++--- ...xi_Ri_noise.hpp => tijsAndXis_From_xi_Ri_noise.hpp} | 4 ++-- ...ty_outlier_test.cpp => tijsAndXis_outlier_test.cpp} | 4 ++-- ...dStructure_L_Infty_test.cpp => tijsAndXis_test.cpp} | 10 +++++----- .../linearProgramming/lInfinityCV/triangulation.hpp | 4 ++-- ...ulation_L_Infty_test.cpp => triangulation_test.cpp} | 6 +++--- 12 files changed, 37 insertions(+), 37 deletions(-) rename src/openMVG/linearProgramming/lInfinityCV/{resection_L_Infty_robust_test.cpp => resection_robust_test.cpp} (94%) rename src/openMVG/linearProgramming/lInfinityCV/{resection_L_Infty_test.cpp => resection_test.cpp} (97%) rename src/openMVG/linearProgramming/lInfinityCV/{translationAndStructureFrom_xi_Ri.hpp => tijsAndXis_From_xi_Ri.hpp} (97%) rename src/openMVG/linearProgramming/lInfinityCV/{translationAndStructureFrom_xi_Ri_noise.hpp => tijsAndXis_From_xi_Ri_noise.hpp} (99%) rename src/openMVG/linearProgramming/lInfinityCV/{translationAndStructure_L_Infty_outlier_test.cpp => tijsAndXis_outlier_test.cpp} (98%) rename src/openMVG/linearProgramming/lInfinityCV/{translationAndStructure_L_Infty_test.cpp => tijsAndXis_test.cpp} (97%) rename src/openMVG/linearProgramming/lInfinityCV/{triangulation_L_Infty_test.cpp => triangulation_test.cpp} (96%) diff --git a/src/openMVG/linearProgramming/lInfinityCV/CMakeLists.txt b/src/openMVG/linearProgramming/lInfinityCV/CMakeLists.txt index ea85a2e211..9f8fd8a5a5 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/CMakeLists.txt +++ b/src/openMVG/linearProgramming/lInfinityCV/CMakeLists.txt @@ -20,15 +20,15 @@ LIST(REMOVE_ITEM lInftycomputervision_cpp ${REMOVEFILESUNITTEST}) ADD_LIBRARY(openMVG_lInftyComputerVision ${lInftycomputervision_headers} ${lInftycomputervision_cpp}) # Triangulation -UNIT_TEST(openMVG_lInftyComputerVision triangulation_L_Infty "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") +UNIT_TEST(openMVG_lInftyCV triangulation "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") # Pose/Resection estimation -UNIT_TEST(openMVG_lInftyComputerVision resection_L_Infty +UNIT_TEST(openMVG_lInftyCV resection "openMVG_multiview_test_data;openMVG_multiview;openMVG_linearProgramming;openMVG_lInftyComputerVision") -UNIT_TEST(openMVG_lInftyComputerVision resection_L_Infty_robust +UNIT_TEST(openMVG_lInftyCV resection_robust "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") # Translation and Structure -UNIT_TEST(openMVG_lInfinityComputerVision translationAndStructure_L_Infty "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") +UNIT_TEST(openMVG_lInftyCV tijsAndXis "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") # Translation and Structure with noise handling -UNIT_TEST(openMVG_lInfinityComputerVision translationAndStructure_L_Infty_outlier "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") +UNIT_TEST(openMVG_lInftyCV tijsAndXis_outlier "openMVG_multiview;openMVG_multiview_test_data;openMVG_linearProgramming;openMVG_lInftyComputerVision") diff --git a/src/openMVG/linearProgramming/lInfinityCV/resection.hpp b/src/openMVG/linearProgramming/lInfinityCV/resection.hpp index a637ec56f1..3803440556 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/resection.hpp +++ b/src/openMVG/linearProgramming/lInfinityCV/resection.hpp @@ -25,7 +25,7 @@ //-- namespace openMVG { -namespace lInfinitycomputervision { +namespace lInfinityCV { using namespace linearProgramming; @@ -186,7 +186,7 @@ struct Resection_L1_ConstraintBuilder Mat3X _3DPt; }; -} // namespace lInfinitycomputervision +} // namespace lInfinityCV } // namespace openMVG #endif // OPENMVG_LINFINITY_COMPUTER_VISION_RESECTION_H_ diff --git a/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.cpp b/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.cpp index 47d3fa8ded..a772a05424 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.cpp +++ b/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.cpp @@ -8,13 +8,13 @@ #include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" #include "openMVG/linearProgramming/bisectionLP.hpp" -#include "openMVG/linearProgramming/lInfinitycomputervision/resection.hpp" -#include "openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp" +#include "openMVG/linearProgramming/lInfinityCV/resection.hpp" +#include "openMVG/linearProgramming/lInfinityCV/resection_kernel.hpp" #include namespace openMVG { -namespace lInfinitycomputervision { +namespace lInfinityCV { namespace kernel { using namespace std; @@ -22,7 +22,7 @@ using namespace std; void translate(const Mat3X & X, const Vec3 & vecTranslation, Mat3X * XPoints) { XPoints->resize(X.rows(), X.cols()); - for (typename Mat3::Index i=0; icol(i) = X.col(i) + vecTranslation; } } @@ -64,5 +64,5 @@ void l1SixPointResectionSolver::Solve(const Mat &pt2D, const Mat &pt3d, vector l1PoseResectionKernel; } // namespace kernel -} // namespace lInfinitycomputervision +} // namespace lInfinityCV } // namespace openMVG #endif // OPENMVG_RESECTION_L1_KERNEL_H_ diff --git a/src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_robust_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/resection_robust_test.cpp similarity index 94% rename from src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_robust_test.cpp rename to src/openMVG/linearProgramming/lInfinityCV/resection_robust_test.cpp index 247657a18f..51792fe047 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_robust_test.cpp +++ b/src/openMVG/linearProgramming/lInfinityCV/resection_robust_test.cpp @@ -8,7 +8,7 @@ #include "CppUnitLite/TestHarness.h" #include "testing/testing.h" -#include "openMVG/linearProgramming/lInfinitycomputervision/resection_kernel.hpp" +#include "openMVG/linearProgramming/lInfinityCV/resection_kernel.hpp" #include "openMVG/robust_estimation/robust_estimator_MaxConsensus.hpp" #include "openMVG/robust_estimation/score_evaluator.hpp" #include "openMVG/multiview/projection.hpp" @@ -36,7 +36,7 @@ TEST(Resection_L_Infinity, Robust_OutlierFree) { // Solve the problem and check that fitted value are good enough { - typedef lInfinitycomputervision::kernel::l1PoseResectionKernel KernelType; + typedef lInfinityCV::kernel::l1PoseResectionKernel KernelType; const Mat & pt2D = d2._x[nResectionCameraIndex]; const Mat & pt3D = d2._X; KernelType kernel(pt2D, pt3D); @@ -90,7 +90,7 @@ TEST(Resection_L_Infinity, Robust_OneOutlier) { // Solve the problem and check that fitted value are good enough { - typedef lInfinitycomputervision::kernel::l1PoseResectionKernel KernelType; + typedef lInfinityCV::kernel::l1PoseResectionKernel KernelType; const Mat & pt2D = d2._x[nResectionCameraIndex]; const Mat & pt3D = d2._X; KernelType kernel(pt2D, pt3D); diff --git a/src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/resection_test.cpp similarity index 97% rename from src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_test.cpp rename to src/openMVG/linearProgramming/lInfinityCV/resection_test.cpp index 31ebb5b7fb..130691c099 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/resection_L_Infty_test.cpp +++ b/src/openMVG/linearProgramming/lInfinityCV/resection_test.cpp @@ -16,17 +16,17 @@ #include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" #include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" #include "openMVG/linearProgramming/bisectionLP.hpp" -#include "openMVG/linearProgramming/lInfinitycomputervision/resection.hpp" +#include "openMVG/linearProgramming/lInfinityCV/resection.hpp" using namespace openMVG; using namespace linearProgramming; -using namespace lInfinitycomputervision; +using namespace lInfinityCV; void translate(const Mat3X & X, const Vec3 & vecTranslation, Mat3X * XPoints) { XPoints->resize(X.rows(), X.cols()); - for (typename Mat3::Index i=0; icol(i) = X.col(i) + vecTranslation; } } diff --git a/src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri.hpp b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri.hpp similarity index 97% rename from src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri.hpp rename to src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri.hpp index 8c36830824..a216141aac 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri.hpp +++ b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri.hpp @@ -24,7 +24,7 @@ //-- namespace openMVG { -namespace lInfinitycomputervision { +namespace lInfinityCV { using namespace linearProgramming; @@ -70,7 +70,7 @@ void EncodeTiXi(const Mat & M, //Scene representation vec_bounds = std::vector< std::pair >(3 * (N3D + Ncam)); fill( vec_bounds.begin(), vec_bounds.end(), std::make_pair((double)-1e+30, (double)1e+30)); // Fix the translation ambiguity. (set first cam at (0,0,0)) - vec_bounds[0] = vec_bounds[1] = vec_bounds[2] = make_pair(0,0); + vec_bounds[0] = vec_bounds[1] = vec_bounds[2] = std::make_pair(0,0); size_t rowPos = 0; // Add the cheirality conditions (R_i*X_j + T_i)_3 + Z_ij >= 1 @@ -185,7 +185,7 @@ struct Translation_Structure_L1_ConstraintBuilder Mat _M; // M contains (X,Y,index3dPoint, indexCam)^T }; -} // namespace lInfinitycomputervision +} // namespace lInfinityCV } // namespace openMVG #endif // OPENMVG_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_H_ diff --git a/src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri_noise.hpp b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp similarity index 99% rename from src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri_noise.hpp rename to src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp index 8390b4a9f1..d463b6c7e3 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/translationAndStructureFrom_xi_Ri_noise.hpp +++ b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp @@ -24,7 +24,7 @@ //-- namespace openMVG { -namespace lInfinitycomputervision { +namespace lInfinityCV { using namespace linearProgramming; @@ -215,7 +215,7 @@ struct TiXi_withNoise_L1_ConstraintBuilder Mat _M; // M contains (X,Y,index3dPoint, indexCam)^T }; -} // namespace lInfinitycomputervision +} // namespace lInfinityCV } // namespace openMVG #endif // OPENMVG_LINFINITY_COMPUTER_VISION_TRANSLATIONANDSTRUCTUREFrom_xi_RI_NOISE_H_ diff --git a/src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_outlier_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_outlier_test.cpp similarity index 98% rename from src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_outlier_test.cpp rename to src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_outlier_test.cpp index 80e05d19fb..90ef121152 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_outlier_test.cpp +++ b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_outlier_test.cpp @@ -15,7 +15,7 @@ #include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" #include "openMVG/linearProgramming/bisectionLP.hpp" -#include "openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri_noise.hpp" +#include "openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp" #include #include @@ -23,7 +23,7 @@ using namespace openMVG; using namespace linearProgramming; -using namespace lInfinitycomputervision; +using namespace lInfinityCV; TEST(Translation_Structure_L_Infinity_Noisy, Outlier_OSICLP_SOLVER) { diff --git a/src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_test.cpp similarity index 97% rename from src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_test.cpp rename to src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_test.cpp index df8b748866..51f9e71d07 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/translationAndStructure_L_Infty_test.cpp +++ b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_test.cpp @@ -16,7 +16,7 @@ #include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" #include "openMVG/linearProgramming/bisectionLP.hpp" -#include "openMVG/linearProgramming/lInfinitycomputervision/translationAndStructureFrom_xi_Ri.hpp" +#include "openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri.hpp" #include #include @@ -24,7 +24,7 @@ using namespace openMVG; using namespace linearProgramming; -using namespace lInfinitycomputervision; +using namespace lInfinityCV; TEST(Translation_Structure_L_Infinity, OSICLP_SOLVER) { @@ -129,10 +129,10 @@ TEST(Translation_Structure_L_Infinity, OSICLP_SOLVER_K) { Mat megaMat(4, d._n*d._x[0].cols()); { size_t cpt = 0; - for (size_t i=0; i Date: Sun, 19 Jan 2014 10:40:05 +0100 Subject: [PATCH 21/68] Update comments --- .../lInfinityCV/tijsAndXis_From_xi_Ri.hpp | 4 ++-- .../lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp | 14 +++++++++++--- .../robust_essential/essential_estimation.hpp | 2 +- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri.hpp b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri.hpp index 417050bedd..2a80eb520c 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri.hpp +++ b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri.hpp @@ -28,8 +28,8 @@ namespace lInfinityCV { using namespace linearProgramming; -//-- Estimate the translation and the structure. -// from image points coordinates (observations). +//-- 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 // diff --git a/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp index 90af11dc16..26d09690e1 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp +++ b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp @@ -28,9 +28,17 @@ namespace lInfinityCV { using namespace linearProgramming; -//-- Estimate the translation and the structure. -// from image points coordinates (observations). -//-- +//-- 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 [1]): diff --git a/src/openMVG_Samples/robust_essential/essential_estimation.hpp b/src/openMVG_Samples/robust_essential/essential_estimation.hpp index 69e26dea77..705d90fe16 100644 --- a/src/openMVG_Samples/robust_essential/essential_estimation.hpp +++ b/src/openMVG_Samples/robust_essential/essential_estimation.hpp @@ -107,7 +107,7 @@ bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2, bOk = true; } else { - std::cout << std::endl << "/!\\There is no right solution," + std::cerr << std::endl << "/!\\There is no right solution," <<" probably intermediate results are not correct or no points" <<" in front of both cameras" << std::endl; bOk = false; From dab88435abec2d3f9937270dc1b97b5975f9a319 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 19 Jan 2014 10:40:29 +0100 Subject: [PATCH 22/68] Set OSI_X verbosity to minimum. --- src/openMVG/linearProgramming/linearProgrammingOSI_X.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/openMVG/linearProgramming/linearProgrammingOSI_X.hpp b/src/openMVG/linearProgramming/linearProgrammingOSI_X.hpp index 70ec099448..74f2bba4ce 100644 --- a/src/openMVG/linearProgramming/linearProgrammingOSI_X.hpp +++ b/src/openMVG/linearProgramming/linearProgrammingOSI_X.hpp @@ -81,6 +81,7 @@ template OSI_X_SolverWrapper::OSI_X_SolverWrapper(int nbParams) : LP_Solver(nbParams) { si = new SOLVERINTERFACE; + si->setLogLevel(0); } template From 69dfb03da155b579567765e902e091b76a381219 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 19 Jan 2014 10:44:05 +0100 Subject: [PATCH 23/68] Add TinyThread library. #59 --- src/third_party/tinythread/README.txt | 130 ++++ src/third_party/tinythread/fast_mutex.h | 248 ++++++++ src/third_party/tinythread/tinythread.cpp | 303 +++++++++ src/third_party/tinythread/tinythread.h | 714 ++++++++++++++++++++++ 4 files changed, 1395 insertions(+) create mode 100644 src/third_party/tinythread/README.txt create mode 100644 src/third_party/tinythread/fast_mutex.h create mode 100644 src/third_party/tinythread/tinythread.cpp create mode 100644 src/third_party/tinythread/tinythread.h diff --git a/src/third_party/tinythread/README.txt b/src/third_party/tinythread/README.txt new file mode 100644 index 0000000000..6b16d86886 --- /dev/null +++ b/src/third_party/tinythread/README.txt @@ -0,0 +1,130 @@ +TinyThread++ v1.1 +================= + +http://tinythreadpp.bitsnbites.eu + + +About +----- + +TinyThread++ is a minimalist, portable, threading library for C++, intended to +make it easy to create multi threaded C++ applications. + +The library is closesly modeled after the C++11 standard, but only a subset is +implemented at the moment. + +See the documentation in the doc/html directory for more information. + + +Using TinyThread++ +------------------ + +To use TinyThread++ in your own project, just add tinythread.cpp and +tinythread.h to your project. In your own code, do: + +#include +using namespace tthread; + +If you wish to use the fast_mutex class, inlude fast_mutex.h: + +#include + + +Building the test programs +-------------------------- + +From the test folder, issue one of the following commands: + +Linux, Mac OS X, OpenSolaris etc: + make (you may need to use gmake on some systems) + +Windows/MinGW: + mingw32-make + +Windows/MS Visual Studio: + nmake /f Makefile.msvc + + +History +------- + +v1.1 - 2012.05.07 + - Added thread::detach(). + +v1.0 - 2010.10.01 + - First non-beta release. + - Made mutex non-recursive (according to spec), and added recursive_mutex. + - General class, code & documentation improvements. + - Added a Makefile for MS Visual Studio. + +v0.9 - 2010.08.10 + - Added preliminary support for this_thread::sleep_for(). + +v0.8 - 2010.07.02 + - Switched from CreateThread() to _beginthreadex() for Win32 (should fix + tiny memory leaks). + - Better standards compliance and some code cleanup. + +v0.7 - 2010.05.17 + - Added this_thread::yield(). + - Replaced the non-standard number_of_processors() function with + thread::hardware_concurrency(), which is part of the C++0x draft. + - The thread::id() class is now more standards compliant (correct namespace + and comparison operators). + +v0.6 - 2010.04.28 + - Added a fast_mutex class (in fast_mutex.h). + - Made the test.cpp application compile under Mac OS X and MinGW/g++ 3.x. + +v0.5 - 2010.03.31 + - Added the thread_local keyword (support for thread-local storage). + - Added a test application to test the API (test.cpp). + - Improved the Doxygen documentation. + +v0.4 - 2010.03.27 + - Added thread::get_id() and this_thread::get_id(). + - Changed the namespace name from tinythread to tthread. + +v0.3 - 2010.03.24 + - Fixed a compiler error for fractal.cpp under MS Visual C++. + - Added colors to the fractal generator. + +v0.2 - 2010.03.23 + - Better C++0x conformance. + - Better documentation. + - New classes: + - lock_guard + - New member functions: + - thread::joinable() + - thread::native_handle() + - mutex::try_lock() + - Added a multi threaded fractal generator test application. + +v0.1 - 2010.03.21 + - Initial release. + + +License +------- + +Copyright (c) 2010-2012 Marcus Geelnard + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. + diff --git a/src/third_party/tinythread/fast_mutex.h b/src/third_party/tinythread/fast_mutex.h new file mode 100644 index 0000000000..4d4b7cc431 --- /dev/null +++ b/src/third_party/tinythread/fast_mutex.h @@ -0,0 +1,248 @@ +/* -*- mode: c++; tab-width: 2; indent-tabs-mode: nil; -*- +Copyright (c) 2010-2012 Marcus Geelnard + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. +*/ + +#ifndef _FAST_MUTEX_H_ +#define _FAST_MUTEX_H_ + +/// @file + +// Which platform are we on? +#if !defined(_TTHREAD_PLATFORM_DEFINED_) + #if defined(_WIN32) || defined(__WIN32__) || defined(__WINDOWS__) + #define _TTHREAD_WIN32_ + #else + #define _TTHREAD_POSIX_ + #endif + #define _TTHREAD_PLATFORM_DEFINED_ +#endif + +// Check if we can support the assembly language level implementation (otherwise +// revert to the system API) +#if (defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))) || \ + (defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64))) || \ + (defined(__GNUC__) && (defined(__ppc__))) + #define _FAST_MUTEX_ASM_ +#else + #define _FAST_MUTEX_SYS_ +#endif + +#if defined(_TTHREAD_WIN32_) + #ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN + #define __UNDEF_LEAN_AND_MEAN + #endif + #include + #ifdef __UNDEF_LEAN_AND_MEAN + #undef WIN32_LEAN_AND_MEAN + #undef __UNDEF_LEAN_AND_MEAN + #endif +#else + #ifdef _FAST_MUTEX_ASM_ + #include + #else + #include + #endif +#endif + +namespace tthread { + +/// Fast mutex class. +/// This is a mutual exclusion object for synchronizing access to shared +/// memory areas for several threads. It is similar to the tthread::mutex class, +/// but instead of using system level functions, it is implemented as an atomic +/// spin lock with very low CPU overhead. +/// +/// The \c fast_mutex class is NOT compatible with the \c condition_variable +/// class (however, it IS compatible with the \c lock_guard class). It should +/// also be noted that the \c fast_mutex class typically does not provide +/// as accurate thread scheduling as a the standard \c mutex class does. +/// +/// Because of the limitations of the class, it should only be used in +/// situations where the mutex needs to be locked/unlocked very frequently. +/// +/// @note The "fast" version of this class relies on inline assembler language, +/// which is currently only supported for 32/64-bit Intel x86/AMD64 and +/// PowerPC architectures on a limited number of compilers (GNU g++ and MS +/// Visual C++). +/// For other architectures/compilers, system functions are used instead. +class fast_mutex { + public: + /// Constructor. +#if defined(_FAST_MUTEX_ASM_) + fast_mutex() : mLock(0) {} +#else + fast_mutex() + { + #if defined(_TTHREAD_WIN32_) + InitializeCriticalSection(&mHandle); + #elif defined(_TTHREAD_POSIX_) + pthread_mutex_init(&mHandle, NULL); + #endif + } +#endif + +#if !defined(_FAST_MUTEX_ASM_) + /// Destructor. + ~fast_mutex() + { + #if defined(_TTHREAD_WIN32_) + DeleteCriticalSection(&mHandle); + #elif defined(_TTHREAD_POSIX_) + pthread_mutex_destroy(&mHandle); + #endif + } +#endif + + /// Lock the mutex. + /// The method will block the calling thread until a lock on the mutex can + /// be obtained. The mutex remains locked until \c unlock() is called. + /// @see lock_guard + inline void lock() + { +#if defined(_FAST_MUTEX_ASM_) + bool gotLock; + do { + gotLock = try_lock(); + if(!gotLock) + { + #if defined(_TTHREAD_WIN32_) + Sleep(0); + #elif defined(_TTHREAD_POSIX_) + sched_yield(); + #endif + } + } while(!gotLock); +#else + #if defined(_TTHREAD_WIN32_) + EnterCriticalSection(&mHandle); + #elif defined(_TTHREAD_POSIX_) + pthread_mutex_lock(&mHandle); + #endif +#endif + } + + /// Try to lock the mutex. + /// The method will try to lock the mutex. If it fails, the function will + /// return immediately (non-blocking). + /// @return \c true if the lock was acquired, or \c false if the lock could + /// not be acquired. + inline bool try_lock() + { +#if defined(_FAST_MUTEX_ASM_) + int oldLock; + #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__)) + asm volatile ( + "movl $1,%%eax\n\t" + "xchg %%eax,%0\n\t" + "movl %%eax,%1\n\t" + : "=m" (mLock), "=m" (oldLock) + : + : "%eax", "memory" + ); + #elif defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64)) + int *ptrLock = &mLock; + __asm { + mov eax,1 + mov ecx,ptrLock + xchg eax,[ecx] + mov oldLock,eax + } + #elif defined(__GNUC__) && (defined(__ppc__)) + int newLock = 1; + asm volatile ( + "\n1:\n\t" + "lwarx %0,0,%1\n\t" + "cmpwi 0,%0,0\n\t" + "bne- 2f\n\t" + "stwcx. %2,0,%1\n\t" + "bne- 1b\n\t" + "isync\n" + "2:\n\t" + : "=&r" (oldLock) + : "r" (&mLock), "r" (newLock) + : "cr0", "memory" + ); + #endif + return (oldLock == 0); +#else + #if defined(_TTHREAD_WIN32_) + return TryEnterCriticalSection(&mHandle) ? true : false; + #elif defined(_TTHREAD_POSIX_) + return (pthread_mutex_trylock(&mHandle) == 0) ? true : false; + #endif +#endif + } + + /// Unlock the mutex. + /// If any threads are waiting for the lock on this mutex, one of them will + /// be unblocked. + inline void unlock() + { +#if defined(_FAST_MUTEX_ASM_) + #if defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__)) + asm volatile ( + "movl $0,%%eax\n\t" + "xchg %%eax,%0\n\t" + : "=m" (mLock) + : + : "%eax", "memory" + ); + #elif defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64)) + int *ptrLock = &mLock; + __asm { + mov eax,0 + mov ecx,ptrLock + xchg eax,[ecx] + } + #elif defined(__GNUC__) && (defined(__ppc__)) + asm volatile ( + "sync\n\t" // Replace with lwsync where possible? + : : : "memory" + ); + mLock = 0; + #endif +#else + #if defined(_TTHREAD_WIN32_) + LeaveCriticalSection(&mHandle); + #elif defined(_TTHREAD_POSIX_) + pthread_mutex_unlock(&mHandle); + #endif +#endif + } + + private: +#if defined(_FAST_MUTEX_ASM_) + int mLock; +#else + #if defined(_TTHREAD_WIN32_) + CRITICAL_SECTION mHandle; + #elif defined(_TTHREAD_POSIX_) + pthread_mutex_t mHandle; + #endif +#endif +}; + +} + +#endif // _FAST_MUTEX_H_ + diff --git a/src/third_party/tinythread/tinythread.cpp b/src/third_party/tinythread/tinythread.cpp new file mode 100644 index 0000000000..690eceea1a --- /dev/null +++ b/src/third_party/tinythread/tinythread.cpp @@ -0,0 +1,303 @@ +/* -*- mode: c++; tab-width: 2; indent-tabs-mode: nil; -*- +Copyright (c) 2010-2012 Marcus Geelnard + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. +*/ + +#include +#include "tinythread.h" + +#if defined(_TTHREAD_POSIX_) + #include + #include +#elif defined(_TTHREAD_WIN32_) + #include +#endif + + +namespace tthread { + +//------------------------------------------------------------------------------ +// condition_variable +//------------------------------------------------------------------------------ +// NOTE 1: The Win32 implementation of the condition_variable class is based on +// the corresponding implementation in GLFW, which in turn is based on a +// description by Douglas C. Schmidt and Irfan Pyarali: +// http://www.cs.wustl.edu/~schmidt/win32-cv-1.html +// +// NOTE 2: Windows Vista actually has native support for condition variables +// (InitializeConditionVariable, WakeConditionVariable, etc), but we want to +// be portable with pre-Vista Windows versions, so TinyThread++ does not use +// Vista condition variables. +//------------------------------------------------------------------------------ + +#if defined(_TTHREAD_WIN32_) + #define _CONDITION_EVENT_ONE 0 + #define _CONDITION_EVENT_ALL 1 +#endif + +#if defined(_TTHREAD_WIN32_) +condition_variable::condition_variable() : mWaitersCount(0) +{ + mEvents[_CONDITION_EVENT_ONE] = CreateEvent(NULL, FALSE, FALSE, NULL); + mEvents[_CONDITION_EVENT_ALL] = CreateEvent(NULL, TRUE, FALSE, NULL); + InitializeCriticalSection(&mWaitersCountLock); +} +#endif + +#if defined(_TTHREAD_WIN32_) +condition_variable::~condition_variable() +{ + CloseHandle(mEvents[_CONDITION_EVENT_ONE]); + CloseHandle(mEvents[_CONDITION_EVENT_ALL]); + DeleteCriticalSection(&mWaitersCountLock); +} +#endif + +#if defined(_TTHREAD_WIN32_) +void condition_variable::_wait() +{ + // Wait for either event to become signaled due to notify_one() or + // notify_all() being called + int result = WaitForMultipleObjects(2, mEvents, FALSE, INFINITE); + + // Check if we are the last waiter + EnterCriticalSection(&mWaitersCountLock); + -- mWaitersCount; + bool lastWaiter = (result == (WAIT_OBJECT_0 + _CONDITION_EVENT_ALL)) && + (mWaitersCount == 0); + LeaveCriticalSection(&mWaitersCountLock); + + // If we are the last waiter to be notified to stop waiting, reset the event + if(lastWaiter) + ResetEvent(mEvents[_CONDITION_EVENT_ALL]); +} +#endif + +#if defined(_TTHREAD_WIN32_) +void condition_variable::notify_one() +{ + // Are there any waiters? + EnterCriticalSection(&mWaitersCountLock); + bool haveWaiters = (mWaitersCount > 0); + LeaveCriticalSection(&mWaitersCountLock); + + // If we have any waiting threads, send them a signal + if(haveWaiters) + SetEvent(mEvents[_CONDITION_EVENT_ONE]); +} +#endif + +#if defined(_TTHREAD_WIN32_) +void condition_variable::notify_all() +{ + // Are there any waiters? + EnterCriticalSection(&mWaitersCountLock); + bool haveWaiters = (mWaitersCount > 0); + LeaveCriticalSection(&mWaitersCountLock); + + // If we have any waiting threads, send them a signal + if(haveWaiters) + SetEvent(mEvents[_CONDITION_EVENT_ALL]); +} +#endif + + +//------------------------------------------------------------------------------ +// POSIX pthread_t to unique thread::id mapping logic. +// Note: Here we use a global thread safe std::map to convert instances of +// pthread_t to small thread identifier numbers (unique within one process). +// This method should be portable across different POSIX implementations. +//------------------------------------------------------------------------------ + +#if defined(_TTHREAD_POSIX_) +static thread::id _pthread_t_to_ID(const pthread_t &aHandle) +{ + static mutex idMapLock; + static std::map idMap; + static unsigned long int idCount(1); + + lock_guard guard(idMapLock); + if(idMap.find(aHandle) == idMap.end()) + idMap[aHandle] = idCount ++; + return thread::id(idMap[aHandle]); +} +#endif // _TTHREAD_POSIX_ + + +//------------------------------------------------------------------------------ +// thread +//------------------------------------------------------------------------------ + +/// Information to pass to the new thread (what to run). +struct _thread_start_info { + void (*mFunction)(void *); ///< Pointer to the function to be executed. + void * mArg; ///< Function argument for the thread function. + thread * mThread; ///< Pointer to the thread object. +}; + +// Thread wrapper function. +#if defined(_TTHREAD_WIN32_) +unsigned WINAPI thread::wrapper_function(void * aArg) +#elif defined(_TTHREAD_POSIX_) +void * thread::wrapper_function(void * aArg) +#endif +{ + // Get thread startup information + _thread_start_info * ti = (_thread_start_info *) aArg; + + try + { + // Call the actual client thread function + ti->mFunction(ti->mArg); + } + catch(...) + { + // Uncaught exceptions will terminate the application (default behavior + // according to C++11) + std::terminate(); + } + + // The thread is no longer executing + lock_guard guard(ti->mThread->mDataMutex); + ti->mThread->mNotAThread = true; + + // The thread is responsible for freeing the startup information + delete ti; + + return 0; +} + +thread::thread(void (*aFunction)(void *), void * aArg) +{ + // Serialize access to this thread structure + lock_guard guard(mDataMutex); + + // Fill out the thread startup information (passed to the thread wrapper, + // which will eventually free it) + _thread_start_info * ti = new _thread_start_info; + ti->mFunction = aFunction; + ti->mArg = aArg; + ti->mThread = this; + + // The thread is now alive + mNotAThread = false; + + // Create the thread +#if defined(_TTHREAD_WIN32_) + mHandle = (HANDLE) _beginthreadex(0, 0, wrapper_function, (void *) ti, 0, &mWin32ThreadID); +#elif defined(_TTHREAD_POSIX_) + if(pthread_create(&mHandle, NULL, wrapper_function, (void *) ti) != 0) + mHandle = 0; +#endif + + // Did we fail to create the thread? + if(!mHandle) + { + mNotAThread = true; + delete ti; + } +} + +thread::~thread() +{ + if(joinable()) + std::terminate(); +} + +void thread::join() +{ + if(joinable()) + { +#if defined(_TTHREAD_WIN32_) + WaitForSingleObject(mHandle, INFINITE); + CloseHandle(mHandle); +#elif defined(_TTHREAD_POSIX_) + pthread_join(mHandle, NULL); +#endif + } +} + +bool thread::joinable() const +{ + mDataMutex.lock(); + bool result = !mNotAThread; + mDataMutex.unlock(); + return result; +} + +void thread::detach() +{ + mDataMutex.lock(); + if(!mNotAThread) + { +#if defined(_TTHREAD_WIN32_) + CloseHandle(mHandle); +#elif defined(_TTHREAD_POSIX_) + pthread_detach(mHandle); +#endif + mNotAThread = true; + } + mDataMutex.unlock(); +} + +thread::id thread::get_id() const +{ + if(!joinable()) + return id(); +#if defined(_TTHREAD_WIN32_) + return id((unsigned long int) mWin32ThreadID); +#elif defined(_TTHREAD_POSIX_) + return _pthread_t_to_ID(mHandle); +#endif +} + +unsigned thread::hardware_concurrency() +{ +#if defined(_TTHREAD_WIN32_) + SYSTEM_INFO si; + GetSystemInfo(&si); + return (int) si.dwNumberOfProcessors; +#elif defined(_SC_NPROCESSORS_ONLN) + return (int) sysconf(_SC_NPROCESSORS_ONLN); +#elif defined(_SC_NPROC_ONLN) + return (int) sysconf(_SC_NPROC_ONLN); +#else + // The standard requires this function to return zero if the number of + // hardware cores could not be determined. + return 0; +#endif +} + + +//------------------------------------------------------------------------------ +// this_thread +//------------------------------------------------------------------------------ + +thread::id this_thread::get_id() +{ +#if defined(_TTHREAD_WIN32_) + return thread::id((unsigned long int) GetCurrentThreadId()); +#elif defined(_TTHREAD_POSIX_) + return _pthread_t_to_ID(pthread_self()); +#endif +} + +} diff --git a/src/third_party/tinythread/tinythread.h b/src/third_party/tinythread/tinythread.h new file mode 100644 index 0000000000..aed7b5856b --- /dev/null +++ b/src/third_party/tinythread/tinythread.h @@ -0,0 +1,714 @@ +/* -*- mode: c++; tab-width: 2; indent-tabs-mode: nil; -*- +Copyright (c) 2010-2012 Marcus Geelnard + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any damages +arising from the use of this software. + +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, subject to the following restrictions: + + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + + 2. Altered source versions must be plainly marked as such, and must not be + misrepresented as being the original software. + + 3. This notice may not be removed or altered from any source + distribution. +*/ + +#ifndef _TINYTHREAD_H_ +#define _TINYTHREAD_H_ + +/// @file +/// @mainpage TinyThread++ API Reference +/// +/// @section intro_sec Introduction +/// TinyThread++ is a minimal, portable implementation of basic threading +/// classes for C++. +/// +/// They closely mimic the functionality and naming of the C++11 standard, and +/// should be easily replaceable with the corresponding std:: variants. +/// +/// @section port_sec Portability +/// The Win32 variant uses the native Win32 API for implementing the thread +/// classes, while for other systems, the POSIX threads API (pthread) is used. +/// +/// @section class_sec Classes +/// In order to mimic the threading API of the C++11 standard, subsets of +/// several classes are provided. The fundamental classes are: +/// @li tthread::thread +/// @li tthread::mutex +/// @li tthread::recursive_mutex +/// @li tthread::condition_variable +/// @li tthread::lock_guard +/// @li tthread::fast_mutex +/// +/// @section misc_sec Miscellaneous +/// The following special keywords are available: #thread_local. +/// +/// For more detailed information (including additional classes), browse the +/// different sections of this documentation. A good place to start is: +/// tinythread.h. + +// Which platform are we on? +#if !defined(_TTHREAD_PLATFORM_DEFINED_) + #if defined(_WIN32) || defined(__WIN32__) || defined(__WINDOWS__) + #define _TTHREAD_WIN32_ + #else + #define _TTHREAD_POSIX_ + #endif + #define _TTHREAD_PLATFORM_DEFINED_ +#endif + +// Platform specific includes +#if defined(_TTHREAD_WIN32_) + #ifndef WIN32_LEAN_AND_MEAN + #define WIN32_LEAN_AND_MEAN + #define __UNDEF_LEAN_AND_MEAN + #endif + #include + #ifdef __UNDEF_LEAN_AND_MEAN + #undef WIN32_LEAN_AND_MEAN + #undef __UNDEF_LEAN_AND_MEAN + #endif +#else + #include + #include + #include + #include +#endif + +// Generic includes +#include + +/// TinyThread++ version (major number). +#define TINYTHREAD_VERSION_MAJOR 1 +/// TinyThread++ version (minor number). +#define TINYTHREAD_VERSION_MINOR 1 +/// TinyThread++ version (full version). +#define TINYTHREAD_VERSION (TINYTHREAD_VERSION_MAJOR * 100 + TINYTHREAD_VERSION_MINOR) + +// Do we have a fully featured C++11 compiler? +#if (__cplusplus > 199711L) || (defined(__STDCXX_VERSION__) && (__STDCXX_VERSION__ >= 201001L)) + #define _TTHREAD_CPP11_ +#endif + +// ...at least partial C++11? +#if defined(_TTHREAD_CPP11_) || defined(__GXX_EXPERIMENTAL_CXX0X__) || defined(__GXX_EXPERIMENTAL_CPP0X__) + #define _TTHREAD_CPP11_PARTIAL_ +#endif + +// Macro for disabling assignments of objects. +#ifdef _TTHREAD_CPP11_PARTIAL_ + #define _TTHREAD_DISABLE_ASSIGNMENT(name) \ + name(const name&) = delete; \ + name& operator=(const name&) = delete; +#else + #define _TTHREAD_DISABLE_ASSIGNMENT(name) \ + name(const name&); \ + name& operator=(const name&); +#endif + +/// @def thread_local +/// Thread local storage keyword. +/// A variable that is declared with the @c thread_local keyword makes the +/// value of the variable local to each thread (known as thread-local storage, +/// or TLS). Example usage: +/// @code +/// // This variable is local to each thread. +/// thread_local int variable; +/// @endcode +/// @note The @c thread_local keyword is a macro that maps to the corresponding +/// compiler directive (e.g. @c __declspec(thread)). While the C++11 standard +/// allows for non-trivial types (e.g. classes with constructors and +/// destructors) to be declared with the @c thread_local keyword, most pre-C++11 +/// compilers only allow for trivial types (e.g. @c int). So, to guarantee +/// portable code, only use trivial types for thread local storage. +/// @note This directive is currently not supported on Mac OS X (it will give +/// a compiler error), since compile-time TLS is not supported in the Mac OS X +/// executable format. Also, some older versions of MinGW (before GCC 4.x) do +/// not support this directive. +/// @hideinitializer + +#if !defined(_TTHREAD_CPP11_) && !defined(thread_local) + #if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__SUNPRO_CC) || defined(__IBMCPP__) + #define thread_local __thread + #else + #define thread_local __declspec(thread) + #endif +#endif + + +/// Main name space for TinyThread++. +/// This namespace is more or less equivalent to the @c std namespace for the +/// C++11 thread classes. For instance, the tthread::mutex class corresponds to +/// the std::mutex class. +namespace tthread { + +/// Mutex class. +/// This is a mutual exclusion object for synchronizing access to shared +/// memory areas for several threads. The mutex is non-recursive (i.e. a +/// program may deadlock if the thread that owns a mutex object calls lock() +/// on that object). +/// @see recursive_mutex +class mutex { + public: + /// Constructor. + mutex() +#if defined(_TTHREAD_WIN32_) + : mAlreadyLocked(false) +#endif + { +#if defined(_TTHREAD_WIN32_) + InitializeCriticalSection(&mHandle); +#else + pthread_mutex_init(&mHandle, NULL); +#endif + } + + /// Destructor. + ~mutex() + { +#if defined(_TTHREAD_WIN32_) + DeleteCriticalSection(&mHandle); +#else + pthread_mutex_destroy(&mHandle); +#endif + } + + /// Lock the mutex. + /// The method will block the calling thread until a lock on the mutex can + /// be obtained. The mutex remains locked until @c unlock() is called. + /// @see lock_guard + inline void lock() + { +#if defined(_TTHREAD_WIN32_) + EnterCriticalSection(&mHandle); + while(mAlreadyLocked) Sleep(1000); // Simulate deadlock... + mAlreadyLocked = true; +#else + pthread_mutex_lock(&mHandle); +#endif + } + + /// Try to lock the mutex. + /// The method will try to lock the mutex. If it fails, the function will + /// return immediately (non-blocking). + /// @return @c true if the lock was acquired, or @c false if the lock could + /// not be acquired. + inline bool try_lock() + { +#if defined(_TTHREAD_WIN32_) + bool ret = (TryEnterCriticalSection(&mHandle) ? true : false); + if(ret && mAlreadyLocked) + { + LeaveCriticalSection(&mHandle); + ret = false; + } + return ret; +#else + return (pthread_mutex_trylock(&mHandle) == 0) ? true : false; +#endif + } + + /// Unlock the mutex. + /// If any threads are waiting for the lock on this mutex, one of them will + /// be unblocked. + inline void unlock() + { +#if defined(_TTHREAD_WIN32_) + mAlreadyLocked = false; + LeaveCriticalSection(&mHandle); +#else + pthread_mutex_unlock(&mHandle); +#endif + } + + _TTHREAD_DISABLE_ASSIGNMENT(mutex) + + private: +#if defined(_TTHREAD_WIN32_) + CRITICAL_SECTION mHandle; + bool mAlreadyLocked; +#else + pthread_mutex_t mHandle; +#endif + + friend class condition_variable; +}; + +/// Recursive mutex class. +/// This is a mutual exclusion object for synchronizing access to shared +/// memory areas for several threads. The mutex is recursive (i.e. a thread +/// may lock the mutex several times, as long as it unlocks the mutex the same +/// number of times). +/// @see mutex +class recursive_mutex { + public: + /// Constructor. + recursive_mutex() + { +#if defined(_TTHREAD_WIN32_) + InitializeCriticalSection(&mHandle); +#else + pthread_mutexattr_t attr; + pthread_mutexattr_init(&attr); + pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); + pthread_mutex_init(&mHandle, &attr); +#endif + } + + /// Destructor. + ~recursive_mutex() + { +#if defined(_TTHREAD_WIN32_) + DeleteCriticalSection(&mHandle); +#else + pthread_mutex_destroy(&mHandle); +#endif + } + + /// Lock the mutex. + /// The method will block the calling thread until a lock on the mutex can + /// be obtained. The mutex remains locked until @c unlock() is called. + /// @see lock_guard + inline void lock() + { +#if defined(_TTHREAD_WIN32_) + EnterCriticalSection(&mHandle); +#else + pthread_mutex_lock(&mHandle); +#endif + } + + /// Try to lock the mutex. + /// The method will try to lock the mutex. If it fails, the function will + /// return immediately (non-blocking). + /// @return @c true if the lock was acquired, or @c false if the lock could + /// not be acquired. + inline bool try_lock() + { +#if defined(_TTHREAD_WIN32_) + return TryEnterCriticalSection(&mHandle) ? true : false; +#else + return (pthread_mutex_trylock(&mHandle) == 0) ? true : false; +#endif + } + + /// Unlock the mutex. + /// If any threads are waiting for the lock on this mutex, one of them will + /// be unblocked. + inline void unlock() + { +#if defined(_TTHREAD_WIN32_) + LeaveCriticalSection(&mHandle); +#else + pthread_mutex_unlock(&mHandle); +#endif + } + + _TTHREAD_DISABLE_ASSIGNMENT(recursive_mutex) + + private: +#if defined(_TTHREAD_WIN32_) + CRITICAL_SECTION mHandle; +#else + pthread_mutex_t mHandle; +#endif + + friend class condition_variable; +}; + +/// Lock guard class. +/// The constructor locks the mutex, and the destructor unlocks the mutex, so +/// the mutex will automatically be unlocked when the lock guard goes out of +/// scope. Example usage: +/// @code +/// mutex m; +/// int counter; +/// +/// void increment() +/// { +/// lock_guard guard(m); +/// ++ counter; +/// } +/// @endcode + +template +class lock_guard { + public: + typedef T mutex_type; + + lock_guard() : mMutex(0) {} + + /// The constructor locks the mutex. + explicit lock_guard(mutex_type &aMutex) + { + mMutex = &aMutex; + mMutex->lock(); + } + + /// The destructor unlocks the mutex. + ~lock_guard() + { + if(mMutex) + mMutex->unlock(); + } + + private: + mutex_type * mMutex; +}; + +/// Condition variable class. +/// This is a signalling object for synchronizing the execution flow for +/// several threads. Example usage: +/// @code +/// // Shared data and associated mutex and condition variable objects +/// int count; +/// mutex m; +/// condition_variable cond; +/// +/// // Wait for the counter to reach a certain number +/// void wait_counter(int targetCount) +/// { +/// lock_guard guard(m); +/// while(count < targetCount) +/// cond.wait(m); +/// } +/// +/// // Increment the counter, and notify waiting threads +/// void increment() +/// { +/// lock_guard guard(m); +/// ++ count; +/// cond.notify_all(); +/// } +/// @endcode +class condition_variable { + public: + /// Constructor. +#if defined(_TTHREAD_WIN32_) + condition_variable(); +#else + condition_variable() + { + pthread_cond_init(&mHandle, NULL); + } +#endif + + /// Destructor. +#if defined(_TTHREAD_WIN32_) + ~condition_variable(); +#else + ~condition_variable() + { + pthread_cond_destroy(&mHandle); + } +#endif + + /// Wait for the condition. + /// The function will block the calling thread until the condition variable + /// is woken by @c notify_one(), @c notify_all() or a spurious wake up. + /// @param[in] aMutex A mutex that will be unlocked when the wait operation + /// starts, an locked again as soon as the wait operation is finished. + template + inline void wait(_mutexT &aMutex) + { +#if defined(_TTHREAD_WIN32_) + // Increment number of waiters + EnterCriticalSection(&mWaitersCountLock); + ++ mWaitersCount; + LeaveCriticalSection(&mWaitersCountLock); + + // Release the mutex while waiting for the condition (will decrease + // the number of waiters when done)... + aMutex.unlock(); + _wait(); + aMutex.lock(); +#else + pthread_cond_wait(&mHandle, &aMutex.mHandle); +#endif + } + + /// Notify one thread that is waiting for the condition. + /// If at least one thread is blocked waiting for this condition variable, + /// one will be woken up. + /// @note Only threads that started waiting prior to this call will be + /// woken up. +#if defined(_TTHREAD_WIN32_) + void notify_one(); +#else + inline void notify_one() + { + pthread_cond_signal(&mHandle); + } +#endif + + /// Notify all threads that are waiting for the condition. + /// All threads that are blocked waiting for this condition variable will + /// be woken up. + /// @note Only threads that started waiting prior to this call will be + /// woken up. +#if defined(_TTHREAD_WIN32_) + void notify_all(); +#else + inline void notify_all() + { + pthread_cond_broadcast(&mHandle); + } +#endif + + _TTHREAD_DISABLE_ASSIGNMENT(condition_variable) + + private: +#if defined(_TTHREAD_WIN32_) + void _wait(); + HANDLE mEvents[2]; ///< Signal and broadcast event HANDLEs. + unsigned int mWaitersCount; ///< Count of the number of waiters. + CRITICAL_SECTION mWaitersCountLock; ///< Serialize access to mWaitersCount. +#else + pthread_cond_t mHandle; +#endif +}; + + +/// Thread class. +class thread { + public: +#if defined(_TTHREAD_WIN32_) + typedef HANDLE native_handle_type; +#else + typedef pthread_t native_handle_type; +#endif + + class id; + + /// Default constructor. + /// Construct a @c thread object without an associated thread of execution + /// (i.e. non-joinable). + thread() : mHandle(0), mNotAThread(true) +#if defined(_TTHREAD_WIN32_) + , mWin32ThreadID(0) +#endif + {} + + /// Thread starting constructor. + /// Construct a @c thread object with a new thread of execution. + /// @param[in] aFunction A function pointer to a function of type: + /// void fun(void * arg) + /// @param[in] aArg Argument to the thread function. + /// @note This constructor is not fully compatible with the standard C++ + /// thread class. It is more similar to the pthread_create() (POSIX) and + /// CreateThread() (Windows) functions. + thread(void (*aFunction)(void *), void * aArg); + + /// Destructor. + /// @note If the thread is joinable upon destruction, @c std::terminate() + /// will be called, which terminates the process. It is always wise to do + /// @c join() before deleting a thread object. + ~thread(); + + /// Wait for the thread to finish (join execution flows). + /// After calling @c join(), the thread object is no longer associated with + /// a thread of execution (i.e. it is not joinable, and you may not join + /// with it nor detach from it). + void join(); + + /// Check if the thread is joinable. + /// A thread object is joinable if it has an associated thread of execution. + bool joinable() const; + + /// Detach from the thread. + /// After calling @c detach(), the thread object is no longer assicated with + /// a thread of execution (i.e. it is not joinable). The thread continues + /// execution without the calling thread blocking, and when the thread + /// ends execution, any owned resources are released. + void detach(); + + /// Return the thread ID of a thread object. + id get_id() const; + + /// Get the native handle for this thread. + /// @note Under Windows, this is a @c HANDLE, and under POSIX systems, this + /// is a @c pthread_t. + inline native_handle_type native_handle() + { + return mHandle; + } + + /// Determine the number of threads which can possibly execute concurrently. + /// This function is useful for determining the optimal number of threads to + /// use for a task. + /// @return The number of hardware thread contexts in the system. + /// @note If this value is not defined, the function returns zero (0). + static unsigned hardware_concurrency(); + + _TTHREAD_DISABLE_ASSIGNMENT(thread) + + private: + native_handle_type mHandle; ///< Thread handle. + mutable mutex mDataMutex; ///< Serializer for access to the thread private data. + bool mNotAThread; ///< True if this object is not a thread of execution. +#if defined(_TTHREAD_WIN32_) + unsigned int mWin32ThreadID; ///< Unique thread ID (filled out by _beginthreadex). +#endif + + // This is the internal thread wrapper function. +#if defined(_TTHREAD_WIN32_) + static unsigned WINAPI wrapper_function(void * aArg); +#else + static void * wrapper_function(void * aArg); +#endif +}; + +/// Thread ID. +/// The thread ID is a unique identifier for each thread. +/// @see thread::get_id() +class thread::id { + public: + /// Default constructor. + /// The default constructed ID is that of thread without a thread of + /// execution. + id() : mId(0) {}; + + id(unsigned long int aId) : mId(aId) {}; + + id(const id& aId) : mId(aId.mId) {}; + + inline id & operator=(const id &aId) + { + mId = aId.mId; + return *this; + } + + inline friend bool operator==(const id &aId1, const id &aId2) + { + return (aId1.mId == aId2.mId); + } + + inline friend bool operator!=(const id &aId1, const id &aId2) + { + return (aId1.mId != aId2.mId); + } + + inline friend bool operator<=(const id &aId1, const id &aId2) + { + return (aId1.mId <= aId2.mId); + } + + inline friend bool operator<(const id &aId1, const id &aId2) + { + return (aId1.mId < aId2.mId); + } + + inline friend bool operator>=(const id &aId1, const id &aId2) + { + return (aId1.mId >= aId2.mId); + } + + inline friend bool operator>(const id &aId1, const id &aId2) + { + return (aId1.mId > aId2.mId); + } + + inline friend std::ostream& operator <<(std::ostream &os, const id &obj) + { + os << obj.mId; + return os; + } + + private: + unsigned long int mId; +}; + + +// Related to - minimal to be able to support chrono. +typedef long long __intmax_t; + +/// Minimal implementation of the @c ratio class. This class provides enough +/// functionality to implement some basic @c chrono classes. +template <__intmax_t N, __intmax_t D = 1> class ratio { + public: + static double _as_double() { return double(N) / double(D); } +}; + +/// Minimal implementation of the @c chrono namespace. +/// The @c chrono namespace provides types for specifying time intervals. +namespace chrono { + /// Duration template class. This class provides enough functionality to + /// implement @c this_thread::sleep_for(). + template > class duration { + private: + _Rep rep_; + public: + typedef _Rep rep; + typedef _Period period; + + /// Construct a duration object with the given duration. + template + explicit duration(const _Rep2& r) : rep_(r) {}; + + /// Return the value of the duration object. + rep count() const + { + return rep_; + } + }; + + // Standard duration types. + typedef duration<__intmax_t, ratio<1, 1000000000> > nanoseconds; ///< Duration with the unit nanoseconds. + typedef duration<__intmax_t, ratio<1, 1000000> > microseconds; ///< Duration with the unit microseconds. + typedef duration<__intmax_t, ratio<1, 1000> > milliseconds; ///< Duration with the unit milliseconds. + typedef duration<__intmax_t> seconds; ///< Duration with the unit seconds. + typedef duration<__intmax_t, ratio<60> > minutes; ///< Duration with the unit minutes. + typedef duration<__intmax_t, ratio<3600> > hours; ///< Duration with the unit hours. +} + +/// The namespace @c this_thread provides methods for dealing with the +/// calling thread. +namespace this_thread { + /// Return the thread ID of the calling thread. + thread::id get_id(); + + /// Yield execution to another thread. + /// Offers the operating system the opportunity to schedule another thread + /// that is ready to run on the current processor. + inline void yield() + { +#if defined(_TTHREAD_WIN32_) + Sleep(0); +#else + sched_yield(); +#endif + } + + /// Blocks the calling thread for a period of time. + /// @param[in] aTime Minimum time to put the thread to sleep. + /// Example usage: + /// @code + /// // Sleep for 100 milliseconds + /// this_thread::sleep_for(chrono::milliseconds(100)); + /// @endcode + /// @note Supported duration types are: nanoseconds, microseconds, + /// milliseconds, seconds, minutes and hours. + template void sleep_for(const chrono::duration<_Rep, _Period>& aTime) + { +#if defined(_TTHREAD_WIN32_) + Sleep(int(double(aTime.count()) * (1000.0 * _Period::_as_double()) + 0.5)); +#else + usleep(int(double(aTime.count()) * (1000000.0 * _Period::_as_double()) + 0.5)); +#endif + } +} + +} + +// Define/macro cleanup +#undef _TTHREAD_DISABLE_ASSIGNMENT + +#endif // _TINYTHREAD_H_ From c36c154db5c2effd5e2a114a0833a1efc210fa9b Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 19 Jan 2014 10:54:09 +0100 Subject: [PATCH 24/68] OpenSource implementation of the ICCV13 paper titled 'Global Fusion of Relative Motions for Robust, Accurate and Scalable Structure from Motion. Authors: Pierre MOULON, Pascal MONASSE and Renaud MARLET.'. #60 --- src/software/CMakeLists.txt | 1 + src/software/globalSfM/CMakeLists.txt | 25 + .../globalSfM/ImageCollection_E_ACRobust.hpp | 68 + .../SfMBundleAdjustmentHelper_tonly.hpp | 88 + src/software/globalSfM/SfMGlobalEngine.cpp | 1936 +++++++++++++++++ src/software/globalSfM/SfMGlobalEngine.hpp | 144 ++ .../SfMGlobalEngine_triplet_t_estimator.hpp | 198 ++ .../globalSfM/SfMGlobal_tij_computation.hpp | 537 +++++ src/software/globalSfM/indexedImageGraph.hpp | 92 + .../globalSfM/indexedImageGraphExport.hpp | 145 ++ src/software/globalSfM/main_GlobalSfM.cpp | 75 + .../main_computeMatchesEssential.cpp | 329 +++ src/software/globalSfM/mutexSet.hpp | 48 + 13 files changed, 3686 insertions(+) create mode 100644 src/software/globalSfM/CMakeLists.txt create mode 100644 src/software/globalSfM/ImageCollection_E_ACRobust.hpp create mode 100644 src/software/globalSfM/SfMBundleAdjustmentHelper_tonly.hpp create mode 100644 src/software/globalSfM/SfMGlobalEngine.cpp create mode 100644 src/software/globalSfM/SfMGlobalEngine.hpp create mode 100644 src/software/globalSfM/SfMGlobalEngine_triplet_t_estimator.hpp create mode 100644 src/software/globalSfM/SfMGlobal_tij_computation.hpp create mode 100644 src/software/globalSfM/indexedImageGraph.hpp create mode 100644 src/software/globalSfM/indexedImageGraphExport.hpp create mode 100644 src/software/globalSfM/main_GlobalSfM.cpp create mode 100644 src/software/globalSfM/main_computeMatchesEssential.cpp create mode 100644 src/software/globalSfM/mutexSet.hpp diff --git a/src/software/CMakeLists.txt b/src/software/CMakeLists.txt index 83047efbb4..0c91179086 100644 --- a/src/software/CMakeLists.txt +++ b/src/software/CMakeLists.txt @@ -1,3 +1,4 @@ +ADD_SUBDIRECTORY(globalSfM) ADD_SUBDIRECTORY(SfM) ADD_SUBDIRECTORY(SfMViewer) #ADD_SUBDIRECTORY(opencv) diff --git a/src/software/globalSfM/CMakeLists.txt b/src/software/globalSfM/CMakeLists.txt new file mode 100644 index 0000000000..5b97563d6a --- /dev/null +++ b/src/software/globalSfM/CMakeLists.txt @@ -0,0 +1,25 @@ + +SET(LIBS ${EXTRA_LIBS} + openMVG_multiview + openMVG_image + vlsift + lemon + stlplus + ceres) + +ADD_EXECUTABLE( + openMVG_main_GlobalSfM + main_GlobalSfM.cpp + SfMGlobalEngine.cpp) +TARGET_LINK_LIBRARIES( + openMVG_main_GlobalSfM + ${LIBS} + openMVG_linearProgramming + openMVG_lInftyComputerVision) + +ADD_EXECUTABLE( + openMVG_main_computeMatchesEssential + main_computeMatchesEssential.cpp) +TARGET_LINK_LIBRARIES(openMVG_main_computeMatchesEssential + ${LIBS}) + diff --git a/src/software/globalSfM/ImageCollection_E_ACRobust.hpp b/src/software/globalSfM/ImageCollection_E_ACRobust.hpp new file mode 100644 index 0000000000..e01365f345 --- /dev/null +++ b/src/software/globalSfM/ImageCollection_E_ACRobust.hpp @@ -0,0 +1,68 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#pragma once + +#include "openMVG/multiview/solver_essential_kernel.hpp" +#include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" +#include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp" + +using namespace openMVG; +using namespace openMVG::robust; + +#include + +//-- A contrario Functor to filter putative corresponding points +//-- thanks estimation of the essential matrix. +//- Suppose that all image have the same K matrix +struct GeometricFilter_EMatrix_AC +{ + GeometricFilter_EMatrix_AC( + const Mat3 & K, + double dPrecision = std::numeric_limits::infinity(), + size_t iteration = 4096) + : m_dPrecision(dPrecision), m_stIteration(iteration), m_K(K) {}; + + /// Robust fitting of the ESSENTIAL matrix + void Fit( + const Mat & xA, + const std::pair & imgSizeA, + const Mat & xB, + const std::pair & imgSizeB, + std::vector & vec_inliers) const + { + vec_inliers.resize(0); + + // Define the AContrario adapted Essential matrix solver + typedef ACKernelAdaptorEssential< + openMVG::essential::kernel::FivePointKernel, + openMVG::fundamental::kernel::EpipolarDistanceError, + UnnormalizerT, + Mat3> + KernelType; + + KernelType kernel(xA, imgSizeA.first, imgSizeA.second, + xB, imgSizeB.first, imgSizeB.second, + m_K, m_K); + + // Robustly estimate the Essential matrix with A Contrario ransac + Mat3 E; + double upper_bound_precision = m_dPrecision; + std::pair ACRansacOut = + ACRANSAC(kernel, vec_inliers, m_stIteration, &E, upper_bound_precision); + const double & threshold = ACRansacOut.first; + const double & NFA = ACRansacOut.second; + + if (vec_inliers.size() < KernelType::MINIMUM_SAMPLES *2.5) { + vec_inliers.clear(); + } + } + + double m_dPrecision; //upper_bound of the precision + size_t m_stIteration; //maximal number of used iterations + Mat3 m_K; // the considered intrinsic matrix +}; diff --git a/src/software/globalSfM/SfMBundleAdjustmentHelper_tonly.hpp b/src/software/globalSfM/SfMBundleAdjustmentHelper_tonly.hpp new file mode 100644 index 0000000000..d43de74140 --- /dev/null +++ b/src/software/globalSfM/SfMBundleAdjustmentHelper_tonly.hpp @@ -0,0 +1,88 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_SFM_BUNDLE_ADJUSTMENT_HELPER_TRANSLATION_ONLY_H +#define OPENMVG_SFM_BUNDLE_ADJUSTMENT_HELPER_TRANSLATION_ONLY_H + +#include "openMVG/bundle_adjustment/pinhole_ceres_functor.hpp" +#include "ceres/ceres.h" +#include "ceres/rotation.h" + +namespace openMVG{ +namespace bundle_adjustment{ + +struct PinholeReprojectionError_t { + PinholeReprojectionError_t(const double* const pos_2dpoint, double focal, const double * r) + : m_focal(focal) + { + m_pos_2dpoint[0] = pos_2dpoint[0]; + m_pos_2dpoint[1] = pos_2dpoint[1]; + + m_rotation[0] = r[0]; + m_rotation[1] = r[1]; + m_rotation[2] = r[2]; + } + + template + bool operator()(const T* const cam_t, + const T* const pos_3dpoint, + T* out_residuals) const { + + T rot[3] = {T(m_rotation[0]), T(m_rotation[1]), T(m_rotation[2])}; + T focal (m_focal); + pinhole_reprojectionError::computeResidual( + &rot[0], // => cam_R + &cam_t[0], // => cam_t + &focal, + pos_3dpoint, + m_pos_2dpoint, + out_residuals ); + + return true; + } + + // Static data + double m_pos_2dpoint[2]; // observation + double m_focal; + double m_rotation[3]; +}; + +struct PinholeReprojectionError_Rt { + PinholeReprojectionError_Rt(const double* const pos_2dpoint, double focal) + : m_focal(focal) + { + m_pos_2dpoint[0] = pos_2dpoint[0]; + m_pos_2dpoint[1] = pos_2dpoint[1]; + } + + template + bool operator()( + const T* const cam_Rt, // [R;t] + const T* const pos_3dpoint, + T* out_residuals) const + { + T focal (m_focal); + pinhole_reprojectionError::computeResidual( + &cam_Rt[0], // => cam_R + &cam_Rt[3], // => cam_t + &focal, + pos_3dpoint, + m_pos_2dpoint, + out_residuals ); + + return true; + } + + // Static data + double m_pos_2dpoint[2]; // observation + double m_focal; +}; + +} // namespace bundle_adjustment +} // namespace openMVG + +#endif // OPENMVG_SFM_BUNDLE_ADJUSTMENT_HELPER_TRANSLATION_ONLY_H diff --git a/src/software/globalSfM/SfMGlobalEngine.cpp b/src/software/globalSfM/SfMGlobalEngine.cpp new file mode 100644 index 0000000000..78faf9f5f5 --- /dev/null +++ b/src/software/globalSfM/SfMGlobalEngine.cpp @@ -0,0 +1,1936 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "openMVG/features/features.hpp" +#include "openMVG/image/image.hpp" +#include "openMVG/matching/indMatch_utils.hpp" +#include "openMVG/matching/indexed_sort.hpp" +#include "openMVG/multiview/triangulation_nview.hpp" + +#include "software/globalSfM/indexedImageGraph.hpp" +#include "software/globalSfM/indexedImageGraphExport.hpp" +#include "software/globalSfM/SfMGlobalEngine.hpp" +#include "software/SfM/SfMIOHelper.hpp" +#include "software/SfM/SfMRobust.hpp" +#include "software/SfM/SfMPlyHelper.hpp" + +#include "openMVG/graph/connectedComponent.hpp" + +#include "software/globalSfM/SfMGlobal_tij_computation.hpp" + +#include "third_party/stlplus3/filesystemSimplified/file_system.hpp" +#include "third_party/vectorGraphics/svgDrawer.hpp" +#include "third_party/stlAddition/stlMap.hpp" +#include "third_party/histogram/histogram.hpp" + +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#ifdef OPENMVG_HAVE_MOSEK +#include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" +#endif + +#include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" +#include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp" + +#undef DYNAMIC +#include "openMVG/bundle_adjustment/problem_data_container.hpp" +#include "openMVG/bundle_adjustment/pinhole_ceres_functor.hpp" +#include "software/globalSfM/SfMBundleAdjustmentHelper_tonly.hpp" + +#include "software/globalSfM/SfMGlobalEngine_triplet_t_estimator.hpp" + +#include "lemon/list_graph.h" +#include + +// Rotation +#include "openMVG/multiview/rotation_averaging.hpp" + +#include "third_party/progress/progress.hpp" +#include "third_party/stlplus3/subsystems/timer.hpp" + +#include +#include +#include +#include +#include + +using namespace openMVG; +using namespace openMVG::graphUtils; +using namespace svg; + +namespace openMVG{ + +typedef SIOPointFeature FeatureT; +typedef std::vector featsT; + +/// Return in radian the rotation amplitude of the given rotation matrix +template +inline double +getRotationMagnitude(const Mat & R) +{ + double cos_theta = 0.5 * (R.trace() - 1.0); + cos_theta = clamp(cos_theta, -1.0, 1.0); + return std::acos(cos_theta); +} + +GlobalReconstructionEngine::GlobalReconstructionEngine(const std::string & sImagePath, + const std::string & sMatchesPath, const std::string & sOutDirectory, bool bHtmlReport) + : ReconstructionEngine(sImagePath, sMatchesPath, sOutDirectory) +{ + _bHtmlReport = bHtmlReport; + if (!stlplus::folder_exists(sOutDirectory)) { + stlplus::folder_create(sOutDirectory); + } + if (_bHtmlReport) + { + _htmlDocStream = auto_ptr( + new htmlDocument::htmlDocumentStream("GlobalReconstructionEngine SFM report.")); + _htmlDocStream->pushInfo( + htmlDocument::htmlMarkup("h1", std::string("Current directory: ") + + sImagePath)); + _htmlDocStream->pushInfo("
"); + } +} + +GlobalReconstructionEngine::~GlobalReconstructionEngine() +{ + ofstream htmlFileStream( string(stlplus::folder_append_separator(_sOutDirectory) + + "Reconstruction_Report.html").c_str()); + htmlFileStream << _htmlDocStream->getDoc(); +} + +void GlobalReconstructionEngine::rotationInference( + std::map< std::pair, std::pair > & map_relatives) +{ + std::cout + << "---------------\n" + << "-- INFERENCE on " << _map_Matches_F.size() << " EGs count.\n" + << "---------------" << std::endl + << " /!\\ /!\\ /!\\ /!\\ /!\\ /!\\ /!\\ \n" + << "--- ITERATED BAYESIAN INFERENCE IS NOT RELEASED, SEE C.ZACH CODE FOR MORE INFORMATION" << std::endl + << " /!\\ /!\\ /!\\ /!\\ /!\\ /!\\ /!\\ \n" << std::endl + << " A simple inference scheme is used here :" << std::endl + << "\t only the relative error composition to identity on cycle of length 3 is used." << std::endl; + + //------------------- + // Triplet inference (test over the composition error) + //------------------- + std::vector< graphUtils::Triplet > vec_triplets; + tripletListing(vec_triplets); + //-- Rejection triplet that are 'not' identity rotation (error to identity > 2°) + tripletRotationRejection(vec_triplets, map_relatives); +} + +bool GlobalReconstructionEngine::computeGlobalRotations( + const std::vector, Mat3> > & vec_relativeRotEstimate, + std::map & map_globalR) const +{ + std::vector vec_ApprRotMatrix; + if (!rotation_averaging::L2RotationAveraging(map_cameraIndexTocameraNode.size(), + vec_relativeRotEstimate, + vec_ApprRotMatrix)) + { + return false; + } + + //-- Setup the averaged rotation + for (int i = 0; i < vec_ApprRotMatrix.size(); ++i) + { + map_globalR[map_cameraIndexTocameraNode.find(i)->second] = vec_ApprRotMatrix[i]; + } + + return true; +} + +bool GlobalReconstructionEngine::Process() +{ + +//--------------------------------- +//-- Global Calibration ----------- +//--------------------------------- + + //------------------- + // Load data + //------------------- + + if(!ReadInputData()) { + std::cout << "\nError while parsing input data" << std::endl; + return false; + } + + //-- Export input graph + { + typedef lemon::ListGraph Graph; + imageGraph::indexedImageGraph putativeGraph(_map_Matches_F, _vec_fileNames); + + // Save the graph before cleaning: + imageGraph::exportToGraphvizData( + stlplus::create_filespec(_sOutDirectory, "input_graph"), + putativeGraph.g); + } + + + stlplus::timer total_reconstruction_timer; + + //------------------- + // Only keep the largest biedge connected subgraph + //------------------- + + if(!CleanGraph()) + return false; + + //------------------- + // Compute relative R|t + //------------------- + + std::map< std::pair, std::pair > map_relatives; + { + ComputeRelativeRt(map_relatives); + } + + //------------------- + // Rotation inference + //------------------- + + //-- Putative triplets for relative translations computation + std::vector< graphUtils::Triplet > vec_triplets; + { + stlplus::timer timer_Inference; + + rotationInference(map_relatives); + + //------------------- + // keep the largest biedge connected subgraph + //------------------- + if(!CleanGraph()) + return false; + + // recompute possible triplets since some nodes have been possibly removed + tripletListing(vec_triplets); + + double time_Inference = timer_Inference.elapsed(); + + //--------------------------------------- + //-- Export geometric filtered matches + //--------------------------------------- + std::ofstream file (string(_sMatchesPath + "/matches.filtered.txt").c_str()); + if (file.is_open()) + PairedIndMatchToStream(_map_Matches_F, file); + file.close(); + + //------------------- + // List remaining camera node Id + //------------------- + std::set set_indeximage; + for (map< std::pair, vector >::const_iterator + iter = _map_Matches_F.begin(); + iter != _map_Matches_F.end(); + ++iter) + { + const size_t I = iter->first.first; + const size_t J = iter->first.second; + set_indeximage.insert(I); + set_indeximage.insert(J); + } + // Build correspondences tables (cameraIds <=> GraphIds) + for (std::set::const_iterator iterSet = set_indeximage.begin(); + iterSet != set_indeximage.end(); ++iterSet) + { + map_cameraIndexTocameraNode[std::distance(set_indeximage.begin(), iterSet)] = *iterSet; + map_cameraNodeToCameraIndex[*iterSet] = std::distance(set_indeximage.begin(), iterSet); + } + + std::cout << "\n Remaining cameras after inference filter : \n" + << map_cameraIndexTocameraNode.size() << " from a total of " << _vec_fileNames.size() << std::endl; + + //-- Export statistics about the rotation inference step: + if (_bHtmlReport) + { + using namespace htmlDocument; + std::ostringstream os; + os << "Rotation inference."; + _htmlDocStream->pushInfo("
"); + _htmlDocStream->pushInfo(htmlMarkup("h1",os.str())); + + os.str(""); + os << "-------------------------------" << "
" + << "-- #Camera count: " << set_indeximage.size() << " remains " + << "-- from " <<_vec_fileNames.size() << " input images.
" + << "-- timing : " << time_Inference << " second
" + << "-------------------------------" << "
"; + _htmlDocStream->pushInfo(os.str()); + } + } + + //---------------------------- + // Rotation averaging (dense) + //---------------------------- + + std::map map_globalR; + + { + // Build relative information for only the largest considered Connected Component + // - it requires to change the index also, because RotationAveraging is working only with + // index ranging in [0 - nbCam]: + std::vector, Mat3> > vec_relativeRotEstimate; + for(std::map, std::pair >::const_iterator iter = map_relatives.begin(); + iter != map_relatives.end(); ++iter) + { + const openMVG::lInfinityCV::relativeInfo & rel = *iter; + //--- + //-- TODO CHANGE INDEXES OF THE CAMERA (from node Ids, to 0->nCam ids) -- + //---------- + if (map_cameraNodeToCameraIndex.find(rel.first.first) != map_cameraNodeToCameraIndex.end() && + map_cameraNodeToCameraIndex.find(rel.first.second) != map_cameraNodeToCameraIndex.end()) + { + std::pair newPair( + map_cameraNodeToCameraIndex[rel.first.first], + map_cameraNodeToCameraIndex[rel.first.second]); + vec_relativeRotEstimate.push_back(std::make_pair(newPair, rel.second.first)); + } + } + + if (!computeGlobalRotations(vec_relativeRotEstimate, map_globalR)) + { + std::cerr << "Failed to compute the global rotations." << std::endl; + return false; + } + } + + //------------------- + // Relative translations estimation (Triplet based translation computation) + //------------------- + std::vector vec_initialRijTijEstimates; + STLPairWiseMatches newpairMatches; + { + // Compute putative translations with an edge coverage algorithm + + stlplus::timer timerLP_triplet; + + computePutativeTranslation_EdgesCoverage(map_globalR, vec_triplets, vec_initialRijTijEstimates, newpairMatches); + double timeLP_triplet = timerLP_triplet.elapsed(); + std::cout << "TRIPLET COVERAGE TIMING : " << timeLP_triplet << " seconds" << std::endl; + + //-- Export triplet statistics: + if (_bHtmlReport) + { + using namespace htmlDocument; + std::ostringstream os; + os << "Triplet statistics."; + _htmlDocStream->pushInfo("
"); + _htmlDocStream->pushInfo(htmlMarkup("h1",os.str())); + + os.str(""); + os << "-------------------------------" << "
" + << "-- #Effective triplet estimates: " << vec_initialRijTijEstimates.size()/3 + << " from " <" + << "-- resulting in " <" + << "-- timing to obtain the relative translations : " << timeLP_triplet << " seconds.
" + << "-------------------------------" << "
"; + _htmlDocStream->pushInfo(os.str()); + } + } + + //--Check relative translation graph : + //--> Consider only the connected component compound by the translation graph + //-- Robust translation estimation can perform inference and remove some bad conditionned triplets + + { + std::set set_representedImageIndex; + for(size_t i = 0; i < vec_initialRijTijEstimates.size(); ++i) + { + const openMVG::lInfinityCV::relativeInfo & rel = vec_initialRijTijEstimates[i]; + set_representedImageIndex.insert(rel.first.first); + set_representedImageIndex.insert(rel.first.second); + } + std::cout << "\n\n" + << "We targeting to estimates : " << map_globalR.size() + << " and we have estimation for : " << set_representedImageIndex.size() << " images" << std::endl; + + //-- PRINT IMAGE THAT ARE NOT INSIDE THE TRIPLET GRAPH + for(std::map::const_iterator iter = map_globalR.begin(); + iter != map_globalR.end(); ++iter) + { + if (set_representedImageIndex.find(iter->first) == set_representedImageIndex.end()) + { + std::cout << "Missing image index: " << iter->first << std::endl; + map_globalR.erase(map_cameraIndexTocameraNode[iter->first]); + } + } + + //- Build the map of camera index and node Ids that are listed by the triplets of translations + map_cameraIndexTocameraNode.clear(); + map_cameraNodeToCameraIndex.clear(); + for (std::set::const_iterator iterSet = set_representedImageIndex.begin(); + iterSet != set_representedImageIndex.end(); ++iterSet) + { + map_cameraIndexTocameraNode[std::distance(set_representedImageIndex.begin(), iterSet)] = *iterSet; + map_cameraNodeToCameraIndex[*iterSet] = std::distance(set_representedImageIndex.begin(), iterSet); + } + + std::cout << "\n Remaining cameras after inference filter : \n" + << map_cameraIndexTocameraNode.size() << " from a total of " << _vec_fileNames.size() << std::endl; + } + + //------------------- + //-- GLOBAL TRANSLATIONS ESTIMATION from initial triplets t_ij guess + //------------------- + + std::map map_camera; + { + const int iNview = map_cameraNodeToCameraIndex.size(); // The cound of remaining nodes in the graph + + std::cout << "\n There is : " << vec_initialRijTijEstimates.size() + << " Initial estimates.\n" + << " For " << iNview << " cameras."<< std::endl; + + //-- Update initial estimates in range [0->Ncam] + for(size_t i = 0; i < vec_initialRijTijEstimates.size(); ++i) + { + lInfinityCV::relativeInfo & rel = vec_initialRijTijEstimates[i]; + std::pair newPair( + map_cameraNodeToCameraIndex[rel.first.first], + map_cameraNodeToCameraIndex[rel.first.second]); + rel.first = newPair; + } + + stlplus::timer timerLP_translation; + clock_t start_timeLP = clock(); + + bool bNormal = false; + double gamma = -1.0; + std::vector vec_solution; + { + vec_solution.resize(iNview*3 + vec_initialRijTijEstimates.size()/3 + 1); + using namespace openMVG::linearProgramming; +#ifdef OPENMVG_HAVE_MOSEK + MOSEK_SolveWrapper solverLP(vec_solution.size()); +#else + OSI_CLP_SolverWrapper solverLP(vec_solution.size()); +#endif + + lInfinityCV::Tifromtij_ConstraintBuilder_OneLambdaPerTrif cstBuilder(vec_initialRijTijEstimates); + + LP_Constraints_Sparse constraint; + //-- Setup constraint and solver + cstBuilder.Build(constraint); + solverLP.setup( constraint ); + //-- + // Solving + bool bFeasible = solverLP.solve(); + std::cout << " \n Feasibility " << bFeasible << std::endl; + //-- + if (bFeasible) + { + solverLP.getSolution(vec_solution); + gamma = vec_solution[vec_solution.size()-1]; + } + } + + double timeLP_translation = timerLP_translation.elapsed(); + double timeLP_translation_clock = double(clock() - start_timeLP) / CLOCKS_PER_SEC; + + //-- Export triplet statistics: + if (_bHtmlReport) + { + using namespace htmlDocument; + std::ostringstream os; + os << "Translation fusion statistics."; + _htmlDocStream->pushInfo("
"); + _htmlDocStream->pushInfo(htmlMarkup("h1",os.str())); + + os.str(""); + os << "-------------------------------" << "
" + << "-- #relative estimates: " << vec_initialRijTijEstimates.size() + << " converge with gamma: " << gamma << ".
" + << " timing: " << timeLP_translation << ".
" + << " timing clocks (s): " << timeLP_translation_clock << ".
" + << "-------------------------------" << "
"; + _htmlDocStream->pushInfo(os.str()); + } + + std::cout << "Found solution:\n"; + std::copy(vec_solution.begin(), vec_solution.end(), std::ostream_iterator(std::cout, " ")); + + std::vector vec_camTranslation(iNview*3,0); + std::copy(&vec_solution[0], &vec_solution[iNview*3], &vec_camTranslation[0]); + + std::vector vec_camRelLambdas(&vec_solution[iNview*3], &vec_solution[iNview*3 + vec_initialRijTijEstimates.size()/3]); + std::cout << "\ncam position: " << std::endl; + std::copy(vec_camTranslation.begin(), vec_camTranslation.end(), std::ostream_iterator(std::cout, " ")); + std::cout << "\ncam Lambdas: " << std::endl; + std::copy(vec_camRelLambdas.begin(), vec_camRelLambdas.end(), std::ostream_iterator(std::cout, " ")); + + // Build a Pinhole camera for each considered Id + std::vector vec_C; + for (size_t i = 0; i < iNview; ++i) + { + Vec3 t(vec_camTranslation[i*3], vec_camTranslation[i*3+1], vec_camTranslation[i*3+2]); + const size_t camNodeId = map_cameraIndexTocameraNode[i]; + const Mat3 & Ri = map_globalR[camNodeId]; + const Mat3 & _K = _vec_intrinsicGroups[0].m_K; // The same K matrix is used by all the camera + map_camera[camNodeId] = PinholeCamera(_K, Ri, t); + //-- Export camera center + vec_C.push_back(map_camera[camNodeId]._C); + } + plyHelper::exportToPly(vec_C, stlplus::create_filespec(_sOutDirectory, "cameraPath", "ply")); + } + + //------------------- + //-- Initial triangulation + //------------------- + std::vector vec_allScenes; + STLMAPTracks map_tracksSelected; + { + // Build tracks from selected triplets (Union of all the validated triplet tracks (newpairMatches)) + // triangulate tracks + // refine translation + { + TracksBuilder tracksBuilder; + tracksBuilder.Build(newpairMatches); + tracksBuilder.Filter(3); + tracksBuilder.ExportToSTL(map_tracksSelected); + + std::cout << std::endl << "Track stats" << std::endl; + { + std::ostringstream osTrack; + //-- Display stats : + // - number of images + // - number of tracks + std::set set_imagesId; + TracksUtilsMap::ImageIdInTracks(map_tracksSelected, set_imagesId); + osTrack << "------------------" << "\n" + << "-- Tracks Stats --" << "\n" + << " Tracks number: " << tracksBuilder.NbTracks() << "\n" + << " Images Id: " << "\n"; + std::copy(set_imagesId.begin(), + set_imagesId.end(), + std::ostream_iterator(osTrack, ", ")); + osTrack << "\n------------------" << "\n"; + + std::map map_Occurence_TrackLength; + TracksUtilsMap::TracksLength(map_tracksSelected, map_Occurence_TrackLength); + osTrack << "TrackLength, Occurrence" << "\n"; + for (std::map::const_iterator iter = map_Occurence_TrackLength.begin(); + iter != map_Occurence_TrackLength.end(); ++iter) { + osTrack << "\t" << iter->first << "\t" << iter->second << "\n"; + } + osTrack << "\n"; + std::cout << osTrack.str(); + } + } + + // Triangulation of all the tracks + vec_allScenes.resize(map_tracksSelected.size()); + { + std::vector vec_residuals; + vec_residuals.resize(map_tracksSelected.size()); + std::set set_idx_to_remove; + + C_Progress_display my_progress_bar_triangulation( map_tracksSelected.size(), + std::cout, + "Initial triangulation:\n"); + +#ifdef USE_OPENMP + #pragma omp parallel for //schedule(dynamic, 1) +#endif + for (int idx = 0; idx < map_tracksSelected.size(); ++idx) + { + STLMAPTracks::const_iterator iterTracks = map_tracksSelected.begin(); + std::advance(iterTracks, idx); + + const submapTrack & subTrack = iterTracks->second; + + // Look to the features required for the triangulation task + size_t index = 0; + Triangulation trianObj; + for (submapTrack::const_iterator iter = subTrack.begin(); iter != subTrack.end(); ++iter, ++index) { + + const size_t imaIndex = iter->first; + const size_t featIndex = iter->second; + const SIOPointFeature & pt = _map_feats[imaIndex][featIndex]; + // Build the P matrix + trianObj.add(map_camera[imaIndex]._P, pt.coords().cast()); + } + + // Compute the 3D point + const Vec3 Xs = trianObj.compute(); + vec_allScenes[idx] = Xs; + if (trianObj.minDepth() < 0) { + set_idx_to_remove.insert(idx); + } + + //-- Compute residual over all the projections + for (submapTrack::const_iterator iter = subTrack.begin(); iter != subTrack.end(); ++iter) { + const size_t imaIndex = iter->first; + const size_t featIndex = iter->second; + const SIOPointFeature & pt = _map_feats[imaIndex][featIndex]; + vec_residuals[idx] = map_camera[imaIndex].Residual(Xs, pt.coords().cast()); + } + +#ifdef USE_OPENMP + #pragma omp critical +#endif + { + ++my_progress_bar_triangulation; + } + } + + //-- Remove useless tracks and 3D points + { + std::vector vec_allScenes_cleaned; + for(size_t i = 0; i < vec_allScenes.size(); ++i) + { + if (find(set_idx_to_remove.begin(), set_idx_to_remove.end(), i) == set_idx_to_remove.end()) + { + vec_allScenes_cleaned.push_back(vec_allScenes[i]); + } + } + vec_allScenes = vec_allScenes_cleaned; + + for( std::set::const_iterator iter = set_idx_to_remove.begin(); + iter != set_idx_to_remove.end(); ++iter) + { + map_tracksSelected.erase(*iter); + } + std::cout << "\n Tracks have been removed : " << set_idx_to_remove.size() << std::endl; + } + plyHelper::exportToPly(vec_allScenes, stlplus::create_filespec(_sOutDirectory, "raw_pointCloud_LP", "ply")); + + { + // Display some statistics of reprojection errors + std::cout << "\n\nResidual statistics:\n" << std::endl; + minMaxMeanMedian(vec_residuals.begin(), vec_residuals.end()); + double min, max, mean, median; + minMaxMeanMedian(vec_residuals.begin(), vec_residuals.end(), min, max, mean, median); + + Histogram histo(0, *max_element(vec_residuals.begin(),vec_residuals.end())*1.1); + histo.Add(vec_residuals.begin(), vec_residuals.end()); + std::cout << std::endl << "Residual Error pixels : " << std::endl << histo.ToString() << std::endl; + + // Histogram between 0 and 10 pixels + { + std::cout << "\n Histogram between 0 and 10 pixels: \n"; + Histogram histo(0, 10, 20); + histo.Add(vec_residuals.begin(), vec_residuals.end()); + std::cout << std::endl << "Residual Error pixels : " << std::endl << histo.ToString() << std::endl; + } + + //-- Export initial triangulation statistics + if (_bHtmlReport) + { + using namespace htmlDocument; + std::ostringstream os; + os << "Initial triangulation statistics."; + _htmlDocStream->pushInfo("
"); + _htmlDocStream->pushInfo(htmlMarkup("h1",os.str())); + + os.str(""); + os << "-------------------------------" << "
" + << "-- #tracks: " << map_tracksSelected.size() << ".
" + << "-- #observation: " << vec_residuals.size() << ".
" + << "-- residual mean (RMSE): " << std::sqrt(mean) << ".
" + << "-------------------------------" << "
"; + _htmlDocStream->pushInfo(os.str()); + } + } + } + } + + //------------------- + //-- Bundle Adjustment on translation and structure + //------------------- + + bundleAdjustment_t_Xi(map_camera, vec_allScenes, map_tracksSelected); + bundleAdjustment_Rt_Xi(map_camera, vec_allScenes, map_tracksSelected); + + //-- Export statistics about the global process + if (_bHtmlReport) + { + using namespace htmlDocument; + std::ostringstream os; + os << "Global calibration summary triangulation statistics."; + _htmlDocStream->pushInfo("
"); + _htmlDocStream->pushInfo(htmlMarkup("h1",os.str())); + + os.str(""); + os << "-------------------------------" << "
" + << "-- Have calibrated: " << map_camera.size() << " from " + << _vec_fileNames.size() << " input images.
" + << "-- The scene contain " << map_tracksSelected.size() << " 3D points.
" + << "-- Total reconstruction time (Inference, global rot, translation fusion, Ba1, Ba2): " + << total_reconstruction_timer.elapsed() << " seconds.
" + << "-------------------------------" << "
"; + _htmlDocStream->pushInfo(os.str()); + } + + std::cout << std::endl + << "-------------------------------" << "\n" + << "-- Have calibrated: " << map_camera.size() << " from " + << _vec_fileNames.size() << " input images.\n" + << "-- The scene contain " << map_tracksSelected.size() << " 3D points.\n" + << "-- Total reconstruction time (Inference, global rot, translation fusion, Ba1, Ba2): " + << total_reconstruction_timer.elapsed() << " seconds.\n" + << "Relative rotations time is excluded\n" + << "-------------------------------" << std::endl; + + + //-- Export data to openMVG format: + { + const std::string & sOutDirectory = stlplus::folder_append_separator(_sOutDirectory) + "SfM_Output"; //Export directory + const std::vector & vec_fileNames = _vec_fileNames; // vector of image filenames + const std::string & sImagePath = _sImagePath; // The images path + const openMVG::tracks::STLMAPTracks & map_reconstructed = map_tracksSelected; // Tracks (Visibility) + bool bExportImage = false; + + bool bOk = true; + if (!stlplus::is_folder(sOutDirectory)) + { + stlplus::folder_create(sOutDirectory); + bOk = stlplus::is_folder(sOutDirectory); + } + + // Create basis directory structure + stlplus::folder_create( stlplus::folder_append_separator(sOutDirectory) + "cameras"); + stlplus::folder_create( stlplus::folder_append_separator(sOutDirectory) + "clouds"); + stlplus::folder_create( stlplus::folder_append_separator(sOutDirectory) + "images"); + + if (bOk && + stlplus::is_folder(stlplus::folder_append_separator(sOutDirectory) + "cameras") && + stlplus::is_folder( stlplus::folder_append_separator(sOutDirectory) + "clouds") && + stlplus::is_folder( stlplus::folder_append_separator(sOutDirectory) + "images") + ) + { + bOk = true; + } + else { + std::cerr << "Cannot access to one of the desired output directory" << std::endl; + } + + if (bOk) + { + //Export Camera as binary files + std::map map_cameratoIndex; + size_t count = 0; + for (std::map::const_iterator iter = + map_camera.begin(); + iter != map_camera.end(); + ++iter) + { + map_cameratoIndex[iter->first] = count; + const Mat34 & PMat = iter->second._P; + std::ofstream file( + stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "cameras", + stlplus::basename_part(vec_fileNames[iter->first]) + ,"bin").c_str(),std::ios::out|std::ios::binary); + file.write((const char*)PMat.data(),(std::streamsize)(3*4)*sizeof(double)); + bOk &= (!file.fail()); + file.close(); + ++count; + } + + //Export 3D point and tracks + + size_t nc = map_camera.size(); + + // Clipping planes (near and far Z depth per view) + std::vector znear(nc, numeric_limits::max()), zfar(nc, 0); + // Cloud + std::ofstream f( + stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "clouds", + "calib", "ply").c_str()); + + if (!f.is_open()) { + std::cerr << "cannot save cloud" << std::endl; + return false; + } + f << "ply\nformat ascii 1.0\ncomment generated by the Global OpenMVG Calibration Engine" << "\n"; + f << "element vertex " << vec_allScenes.size() << "\n"; + f << "property float x\nproperty float y\nproperty float z" << "\n"; + f << "property uchar red\nproperty uchar green\nproperty uchar blue" << "\n"; + f << "property float confidence\nproperty list uchar int visibility" << "\n"; + f << "element face 0\nproperty list uchar int vertex_index" << "\n"; + f << "end_header" << "\n"; + size_t i = 0; + for (openMVG::tracks::STLMAPTracks::const_iterator iter = map_reconstructed.begin(); + iter != map_reconstructed.end(); ++iter, ++i) + { + // Look through the track and add point position + const tracks::submapTrack & track = iter->second; + + Vec3 pos = vec_allScenes[i]; + + f << pos.transpose() << " " << "255 255 255" << " " << 3.14; + + std::set< size_t > set_imageIndex; + for( tracks::submapTrack::const_iterator iterTrack = track.begin(); + iterTrack != track.end(); + ++iterTrack) + { + size_t imageId = iterTrack->first; + + if ( map_cameratoIndex.find(imageId) != map_cameratoIndex.end()) + { + set_imageIndex.insert(map_cameratoIndex[imageId]); + const PinholeCamera & cam = (map_camera.find(imageId))->second; + double z = Depth(cam._R, cam._t, pos); + znear[map_cameratoIndex[imageId]] = std::min(znear[map_cameratoIndex[imageId]], z ); + zfar[map_cameratoIndex[imageId]] = std::max(zfar[map_cameratoIndex[imageId]], z ); + } + } + + //export images indexes + f << " " << set_imageIndex.size() << " "; + copy(set_imageIndex.begin(), set_imageIndex.end(), std::ostream_iterator(f, " ")); + f << std::endl; + } + f.close(); + + // Views + f.open(stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory), + "views", "txt").c_str()); + if (!f.is_open()) { + std::cerr << "Cannot write views" << endl; + return false; + } + f << "images\ncameras\n" << nc << endl; + + count = 0; + for (std::map::const_iterator iter = map_camera.begin(); + iter != map_camera.end(); + ++iter) + { + size_t camIndex = iter->first; + f << vec_fileNames[camIndex] + << ' ' << _vec_intrinsicGroups[_vec_camImageNames[camIndex].m_intrinsicId].m_w + << ' ' << _vec_intrinsicGroups[_vec_camImageNames[camIndex].m_intrinsicId].m_h + << ' ' << stlplus::basename_part(vec_fileNames[camIndex]) << ".bin" + << ' ' << znear[count]/2 + << ' ' << zfar[count]*2 + << std::endl; + ++count; + } + f.close(); + + // EXPORT IMAGES + if (bExportImage) + { + for (std::map::const_iterator iter = map_camera.begin(); + iter != map_camera.end(); + ++iter) + { + size_t imageIndex = iter->first; + std::string sImageName = vec_fileNames[imageIndex]; + stlplus::file_copy(stlplus::create_filespec(sImagePath,sImageName), + stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "images", + stlplus::basename_part(sImageName), + stlplus::extension_part(sImageName))); + } + } + } + } + return true; +} + +bool testIntrinsicsEquality( + SfMIO::IntrinsicCameraInfo const &ci1, + SfMIO::IntrinsicCameraInfo const &ci2) +{ + return ci1.m_K == ci2.m_K; +} + +bool GlobalReconstructionEngine::ReadInputData() +{ + if (!stlplus::is_folder(_sImagePath) || + !stlplus::is_folder(_sMatchesPath) || + !stlplus::is_folder(_sOutDirectory)) + { + std::cerr << std::endl + << "One of the required directory is not a valid directory" << std::endl; + return false; + } + + // a. Read images names + std::string sListsFile = stlplus::create_filespec(_sMatchesPath,"lists","txt"); + std::string sComputedMatchesFile_E = stlplus::create_filespec(_sMatchesPath,"matches.e","txt"); + if (!stlplus::is_file(sListsFile)|| + !stlplus::is_file(sComputedMatchesFile_E) ) + { + std::cerr << std::endl + << "One of the input required file is not a present (lists.txt, matches.e.txt)" << std::endl; + return false; + } + + // a. Read images names + { + if (!openMVG::SfMIO::loadImageList( _vec_camImageNames, + _vec_intrinsicGroups, + sListsFile) ) + { + std::cerr << "\nEmpty image list." << std::endl; + return false; + } + else + { + // Check there is only one intrinsic group + std::vector::iterator iterF = + std::unique(_vec_intrinsicGroups.begin(), _vec_intrinsicGroups.end(), testIntrinsicsEquality); + _vec_intrinsicGroups.resize( std::distance(_vec_intrinsicGroups.begin(), iterF) ); + if (_vec_intrinsicGroups.size() == 1) + { + for (size_t i = 0; i < _vec_camImageNames.size(); ++i) + _vec_camImageNames[i].m_intrinsicId = 0; + } + else + { + std::cout << "There is more than one focal group in the lists.txt file." << std::endl + << "Only one focal group is supported for the global calibration chain" << std::endl; + return false; + } + + for (size_t i = 0; i < _vec_camImageNames.size(); ++i) + { + _vec_fileNames.push_back( stlplus::create_filespec( _sImagePath, _vec_camImageNames[i].m_sImageName) ); + } + } + } + + // b. Read matches (Essential) + if (!matching::PairedIndMatchImport(sComputedMatchesFile_E, _map_Matches_F)) { + std::cerr<< "Unable to read the Essential matrix matches" << std::endl; + return false; + } + + // Read features: + for (size_t i = 0; i < _vec_fileNames.size(); ++i) { + const size_t camIndex = i; + if (!loadFeatsFromFile( + stlplus::create_filespec(_sMatchesPath, stlplus::basename_part(_vec_fileNames[camIndex]), ".feat"), + _map_feats[camIndex])) { + std::cerr << "Bad reading of feature files" << std::endl; + return false; + } + } + return true; +} + +bool GlobalReconstructionEngine::CleanGraph() +{ + // Create a graph from pairwise correspondences: + // - remove not biedge connected component, + // - keep the largest connected component. + + typedef lemon::ListGraph Graph; + imageGraph::indexedImageGraph putativeGraph(_map_Matches_F, _vec_fileNames); + + // Save the graph before cleaning: + imageGraph::exportToGraphvizData( + stlplus::create_filespec(_sOutDirectory, "initialGraph"), + putativeGraph.g); + + // Remove not bi-edge connected edges + typedef Graph::EdgeMap EdgeMapAlias; + EdgeMapAlias cutMap(putativeGraph.g); + + if (lemon::biEdgeConnectedCutEdges(putativeGraph.g, cutMap) > 0) + { + typedef Graph::EdgeIt EdgeIterator; + EdgeIterator itEdge(putativeGraph.g); + for (EdgeMapAlias::MapIt it(cutMap); it!=INVALID; ++it, ++itEdge) + { + if (*it) // we have to remove an edge + { + putativeGraph.g.erase(itEdge); + size_t Idu = (*putativeGraph.map_nodeMapIndex)[putativeGraph.g.u(itEdge)]; + size_t Idv = (*putativeGraph.map_nodeMapIndex)[putativeGraph.g.v(itEdge)]; + STLPairWiseMatches::iterator iterF = _map_Matches_F.find(std::make_pair(Idu,Idv)); + if( iterF != _map_Matches_F.end()) + { + _map_Matches_F.erase(iterF); + } + iterF = _map_Matches_F.find(std::make_pair(Idv,Idu)); + if( iterF != _map_Matches_F.end()) + { + _map_Matches_F.erase(iterF); + } + } + } + } + // Graph is bi-edge connected, but still many connected components can exist + int connectedComponentCount = lemon::countConnectedComponents(putativeGraph.g); + std::cout << "\n" + << "GlobalReconstructionEngine::CleanGraph() :: => connected Component : " << connectedComponentCount << std::endl; + if (connectedComponentCount > 1) + { + // Keep only the largest connected component + const std::map > map_subgraphs = exportGraphToMapSubgraphs(putativeGraph.g); + size_t count = std::numeric_limits::min(); + std::map >::const_iterator iterLargestCC = map_subgraphs.end(); + for(std::map >::const_iterator iter = map_subgraphs.begin(); + iter != map_subgraphs.end(); ++iter) + { + if (iter->second.size() > count) { + count = iter->second.size(); + iterLargestCC = iter; + } + std::cout << "Connected component of size : " << iter->second.size() << std::endl; + } + + //-- Remove all nodes that are not listed in the largest CC + for(std::map >::const_iterator iter = map_subgraphs.begin(); + iter != map_subgraphs.end(); ++iter) + { + if (iter == iterLargestCC) + continue; + + const std::set & ccSet = iter->second; + for (std::set::const_iterator iter2 = ccSet.begin(); + iter2 != ccSet.end(); ++iter2) + { + //putativeGraph.g.erase(*iter2); + // Remove all outgoing edges + typedef Graph::OutArcIt OutArcIt; + for (OutArcIt e(putativeGraph.g, *iter2); e!=INVALID; ++e) + { + putativeGraph.g.erase(e); + size_t Idu = (*putativeGraph.map_nodeMapIndex)[putativeGraph.g.target(e)]; + size_t Idv = (*putativeGraph.map_nodeMapIndex)[putativeGraph.g.source(e)]; + STLPairWiseMatches::iterator iterF = _map_Matches_F.find(std::make_pair(Idu,Idv)); + if( iterF != _map_Matches_F.end()) + { + _map_Matches_F.erase(iterF); + } + iterF = _map_Matches_F.find(std::make_pair(Idv,Idu)); + if( iterF != _map_Matches_F.end()) + { + _map_Matches_F.erase(iterF); + } + } + } + } + } + + // Save the graph after cleaning: + imageGraph::exportToGraphvizData( + stlplus::create_filespec(_sOutDirectory, "cleanedGraph"), + putativeGraph.g); + + std::cout << "\n" + << "Cardinal of nodes: " << lemon::countNodes(putativeGraph.g) << "\n" + << "Cardinal of edges: " << lemon::countEdges(putativeGraph.g) << std::endl + << std::endl; + + return true; +} + +void GlobalReconstructionEngine::ComputeRelativeRt( + std::map< std::pair, std::pair > & vec_relatives) +{ + // For each pair, compute the rotation from pairwise point matches: + + C_Progress_display my_progress_bar( _map_Matches_F.size(), std::cout, "\n", " " , "ComputeRelativeRt\n" ); +#ifdef USE_OPENMP + #pragma omp parallel for schedule(dynamic) +#endif + for (int i = 0; i < _map_Matches_F.size(); ++i) + { + map< std::pair, vector >::const_iterator iter = _map_Matches_F.begin(); + std::advance(iter, i); + + const size_t I = iter->first.first; + const size_t J = iter->first.second; + + const std::vector & vec_matchesInd = iter->second; + + Mat x1(2, vec_matchesInd.size()), x2(2, vec_matchesInd.size()); + for (size_t k = 0; k < vec_matchesInd.size(); ++k) + { + x1.col(k) = _map_feats[I][vec_matchesInd[k]._i].coords().cast(); + x2.col(k) = _map_feats[J][vec_matchesInd[k]._j].coords().cast(); + } + + Mat3 E; + std::vector vec_inliers; + + std::pair imageSize_I( + _vec_intrinsicGroups[_vec_camImageNames[I].m_intrinsicId].m_w, + _vec_intrinsicGroups[_vec_camImageNames[I].m_intrinsicId].m_h); + + std::pair imageSize_J( + _vec_intrinsicGroups[_vec_camImageNames[J].m_intrinsicId].m_w, + _vec_intrinsicGroups[_vec_camImageNames[J].m_intrinsicId].m_h); + + const Mat3 K = _vec_intrinsicGroups[_vec_camImageNames[I].m_intrinsicId].m_K; + + double errorMax = std::numeric_limits::max(); + double maxExpectedError = 2.5; + if (!SfMRobust::robustEssential(K, K, + x1, x2, + &E, &vec_inliers, + imageSize_I, imageSize_J, + &errorMax, + maxExpectedError)) + { + std::cerr << " /!\\ Robust estimation failed to compute E for this pair" + << std::endl; + continue; + } + else + { + //--> Estimate the best possible Rotation/Translation from E + Mat3 R; + Vec3 t; + if (!SfMRobust::estimate_Rt_fromE(K, K, x1, x2, E, vec_inliers, &R, &t)) + { + std::cout << " /!\\ Failed to compute initial R|t for the initial pair" + << std::endl; + continue; + } + else + { + PinholeCamera cam1(K, Mat3::Identity(), Vec3::Zero()); + PinholeCamera cam2(K, R, t); + + std::vector vec_allScenes; + vec_allScenes.resize(x1.cols()); + for (size_t k = 0; k < x1.cols(); ++k) { + const Vec2 & x1_ = x1.col(k), + & x2_ = x2.col(k); + Vec3 X; + TriangulateDLT(cam1._P, x1_, cam2._P, x2_, &X); + vec_allScenes[k] = X; + } + + // Refine Xis, tis and Ris (Bundle Adjustment) + { + using namespace std; + + const size_t nbCams = 2; + const size_t nbPoints3D = vec_allScenes.size(); + + // Count the number of measurement (sum of the reconstructed track length) + size_t nbmeasurements = x1.cols() * 2; + + // Setup a BA problem + using namespace openMVG::bundle_adjustment; + BA_Problem_data<6> ba_problem; // Will refine translation and 3D points + + // Configure the size of the problem + ba_problem.num_cameras_ = nbCams; + ba_problem.num_points_ = nbPoints3D; + ba_problem.num_observations_ = nbmeasurements; + + ba_problem.point_index_.reserve(ba_problem.num_observations_); + ba_problem.camera_index_.reserve(ba_problem.num_observations_); + ba_problem.observations_.reserve(2 * ba_problem.num_observations_); + + ba_problem.num_parameters_ = 6 * ba_problem.num_cameras_ + 3 * ba_problem.num_points_; + ba_problem.parameters_.reserve(ba_problem.num_parameters_); + + // Fill camera + { + Mat3 R = cam1._R; + double angleAxis[3]; + ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis); + + // translation + Vec3 t = cam1._t; + + ba_problem.parameters_.push_back(angleAxis[0]); + ba_problem.parameters_.push_back(angleAxis[1]); + ba_problem.parameters_.push_back(angleAxis[2]); + ba_problem.parameters_.push_back(t[0]); + ba_problem.parameters_.push_back(t[1]); + ba_problem.parameters_.push_back(t[2]); + } + { + Mat3 R = cam2._R; + double angleAxis[3]; + ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis); + + // translation + Vec3 t = cam2._t; + + ba_problem.parameters_.push_back(angleAxis[0]); + ba_problem.parameters_.push_back(angleAxis[1]); + ba_problem.parameters_.push_back(angleAxis[2]); + ba_problem.parameters_.push_back(t[0]); + ba_problem.parameters_.push_back(t[1]); + ba_problem.parameters_.push_back(t[2]); + } + + // Fill 3D points + for (std::vector::const_iterator iter = vec_allScenes.begin(); + iter != vec_allScenes.end(); + ++iter) + { + const Vec3 & pt3D = *iter; + ba_problem.parameters_.push_back(pt3D[0]); + ba_problem.parameters_.push_back(pt3D[1]); + ba_problem.parameters_.push_back(pt3D[2]); + } + + // Fill the measurements + for (size_t k = 0; k < x1.cols(); ++k) { + const Vec2 & x1_ = x1.col(k), & x2_ = x2.col(k); + + const Mat3 & K = cam1._K; + + double ppx = K(0,2), ppy = K(1,2); + ba_problem.observations_.push_back( x1_(0) - ppx ); + ba_problem.observations_.push_back( x1_(1) - ppy ); + ba_problem.point_index_.push_back(k); + ba_problem.camera_index_.push_back(0); + + ba_problem.observations_.push_back( x2_(0) - ppx ); + ba_problem.observations_.push_back( x2_(1) - ppy ); + ba_problem.point_index_.push_back(k); + ba_problem.camera_index_.push_back(1); + } + + // Create residuals for each observation in the bundle adjustment problem. The + // parameters for cameras and points are added automatically. + ceres::Problem problem; + for (size_t i = 0; i < ba_problem.num_observations(); ++i) { + // Each Residual block takes a point and a camera as input and outputs a 2 + // dimensional residual. Internally, the cost function stores the observed + // image location and compares the reprojection against the observation. + + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction( + new PinholeReprojectionError_Rt( + &ba_problem.observations()[2 * i + 0], K(0,0))); + + problem.AddResidualBlock(cost_function, + new ceres::HuberLoss(Square(2.0)), + //NULL, // squared loss + ba_problem.mutable_camera_for_observation(i), + ba_problem.mutable_point_for_observation(i)); + } + + // Configure a BA engine and run it + // Make Ceres automatically detect the bundle structure. + ceres::Solver::Options options; + options.linear_solver_type = ceres::SPARSE_SCHUR; + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::SUITE_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; + else + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::CX_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::CX_SPARSE; + else + { + // No sparse backend for Ceres. + // Use dense solving + options.linear_solver_type = ceres::DENSE_SCHUR; + } + options.minimizer_progress_to_stdout = false; + options.logging_type = ceres::SILENT; + + // Solve BA + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // If no error, get back refined parameters + if (summary.termination_type != ceres::DID_NOT_RUN && + summary.termination_type != ceres::USER_ABORT && + summary.termination_type != ceres::NUMERICAL_FAILURE) + { + // Get back 3D points + size_t k = 0; + for (std::vector::iterator iter = vec_allScenes.begin(); + iter != vec_allScenes.end(); ++iter, ++k) + { + const double * pt = ba_problem.mutable_points() + k*3; + Vec3 & pt3D = *iter; + pt3D = Vec3(pt[0], pt[1], pt[2]); + } + + // Get back camera 1 + { + const double * cam = ba_problem.mutable_cameras() + 0*6; + Mat3 R; + // angle axis to rotation matrix + ceres::AngleAxisToRotationMatrix(cam, R.data()); + + Vec3 t(cam[3], cam[4], cam[5]); + double focal = cam[6]; + + // Update the camera + Mat3 K = cam1._K; + PinholeCamera & sCam = cam1; + sCam = PinholeCamera(K, R, t); + } + // Get back camera 2 + { + const double * cam = ba_problem.mutable_cameras() + 1*6; + Mat3 R; + // angle axis to rotation matrix + ceres::AngleAxisToRotationMatrix(cam, R.data()); + + Vec3 t(cam[3], cam[4], cam[5]); + double focal = cam[6]; + + // Update the camera + Mat3 K = cam2._K; + PinholeCamera & sCam = cam2; + sCam = PinholeCamera(K, R, t); + } + RelativeCameraMotion(cam1._R, cam1._t, cam2._R, cam2._t, &R, &t); + } + } + +#ifdef USE_OPENMP + #pragma omp critical +#endif + { + vec_relatives[iter->first] = std::make_pair(R,t); + ++my_progress_bar; + } + + } + } + } +} + +void GlobalReconstructionEngine::tripletListing(std::vector< graphUtils::Triplet > & vec_triplets) const +{ + vec_triplets.clear(); + + imageGraph::indexedImageGraph putativeGraph(_map_Matches_F, _vec_fileNames); + + List_Triplets(putativeGraph.g, vec_triplets); + + //Change triplets to ImageIds + for (size_t i = 0; i < vec_triplets.size(); ++i) + { + graphUtils::Triplet & triplet = vec_triplets[i]; + size_t I = triplet.i, J = triplet.j , K = triplet.k; + I = (*putativeGraph.map_nodeMapIndex)[putativeGraph.g.nodeFromId(I)]; + J = (*putativeGraph.map_nodeMapIndex)[putativeGraph.g.nodeFromId(J)]; + K = (*putativeGraph.map_nodeMapIndex)[putativeGraph.g.nodeFromId(K)]; + size_t triplet_[3] = {I,J,K}; + std::sort(&triplet_[0], &triplet_[3]); + triplet = graphUtils::Triplet(triplet_[0],triplet_[1],triplet_[2]); + } +} + +void GlobalReconstructionEngine::tripletRotationRejection( + std::vector< graphUtils::Triplet > & vec_triplets, + std::map< std::pair, std::pair > & map_relatives) +{ + std::map< std::pair, std::pair > map_relatives_validated; + + // DETECTION OF ROTATION OUTLIERS + std::vector< graphUtils::Triplet > vec_triplets_validated; + + std::vector vec_errToIdentityPerTriplet; + vec_errToIdentityPerTriplet.reserve(vec_triplets.size()); + // Compute for each length 3 cycles: the composition error + // Error to identity rotation. + for (size_t i = 0; i < vec_triplets.size(); ++i) + { + const graphUtils::Triplet & triplet = vec_triplets[i]; + size_t I = triplet.i, J = triplet.j , K = triplet.k; + + //-- Find the three rotations + const std::pair ij = std::make_pair(I,J); + const std::pair ji = std::make_pair(J,I); + + Mat3 RIJ; + if (map_relatives.find(ij) != map_relatives.end()) + RIJ = map_relatives.find(ij)->second.first; + else + RIJ = map_relatives.find(ji)->second.first.transpose(); + + const std::pair jk = std::make_pair(J,K); + const std::pair kj = std::make_pair(K,J); + + Mat3 RJK; + if (map_relatives.find(jk) != map_relatives.end()) + RJK = map_relatives.find(jk)->second.first; + else + RJK = map_relatives.find(kj)->second.first.transpose(); + + const std::pair ki = std::make_pair(K,I); + const std::pair ik = std::make_pair(I,K); + + Mat3 RKI; + if (map_relatives.find(ki) != map_relatives.end()) + RKI = map_relatives.find(ki)->second.first; + else + RKI = map_relatives.find(ik)->second.first.transpose(); + + Mat3 Rot_To_Identity = RIJ * RJK * RKI; // motion composition + double angularErrorDegree = R2D(getRotationMagnitude(Rot_To_Identity)); + vec_errToIdentityPerTriplet.push_back(angularErrorDegree); + + if (angularErrorDegree < 2.0) + { + vec_triplets_validated.push_back(triplet); + + if (map_relatives.find(ij) != map_relatives.end()) + map_relatives_validated[ij] = map_relatives.find(ij)->second; + else + map_relatives_validated[ji] = map_relatives.find(ji)->second; + + if (map_relatives.find(jk) != map_relatives.end()) + map_relatives_validated[jk] = map_relatives.find(jk)->second; + else + map_relatives_validated[kj] = map_relatives.find(kj)->second; + + if (map_relatives.find(ki) != map_relatives.end()) + map_relatives_validated[ki] = map_relatives.find(ki)->second; + else + map_relatives_validated[ik] = map_relatives.find(ik)->second; + } + } + + map_relatives = map_relatives_validated; + + // Display statistics about rotation triplets error: + std::cout << "\nStatistics about rotation triplets:" << std::endl; + minMaxMeanMedian(vec_errToIdentityPerTriplet.begin(), vec_errToIdentityPerTriplet.end()); + + std::sort(vec_errToIdentityPerTriplet.begin(), vec_errToIdentityPerTriplet.end()); + + Histogram histo(0.0, *max_element(vec_errToIdentityPerTriplet.begin(), vec_errToIdentityPerTriplet.end()), 180); + histo.Add(vec_errToIdentityPerTriplet.begin(), vec_errToIdentityPerTriplet.end()); + + svgHisto histosvg; + histosvg.draw(histo.GetHist(), + std::make_pair(0.0, *max_element(vec_errToIdentityPerTriplet.begin(), vec_errToIdentityPerTriplet.end())), + stlplus::create_filespec(this->_sOutDirectory, "Triplet_Rotation_Residual_180.svg"), + 600,300); + + histo = Histogram(0.0, *max_element(vec_errToIdentityPerTriplet.begin(), vec_errToIdentityPerTriplet.end()), 20); + histo.Add(vec_errToIdentityPerTriplet.begin(), vec_errToIdentityPerTriplet.end()); + + histosvg.draw(histo.GetHist(), + std::make_pair(0.0, *max_element(vec_errToIdentityPerTriplet.begin(), vec_errToIdentityPerTriplet.end())), + stlplus::create_filespec(this->_sOutDirectory, "Triplet_Rotation_Residual_20.svg"), + 600,300); + + typedef lemon::ListGraph Graph; + imageGraph::indexedImageGraph putativeGraph(_map_Matches_F, _vec_fileNames); + + Graph::EdgeMap edge_filter(putativeGraph.g, false); + Graph::NodeMap node_filter(putativeGraph.g, true); + + typedef SubGraph subGraphT; + subGraphT sg(putativeGraph.g, node_filter, edge_filter); + + // Look all edges of the graph and look if exist in one triplet + for (Graph::EdgeIt iter(putativeGraph.g); iter!=INVALID; ++iter) + { + size_t Idu = (*putativeGraph.map_nodeMapIndex)[sg.u(iter)]; + size_t Idv = (*putativeGraph.map_nodeMapIndex)[sg.v(iter)]; + //-- Look if the edge Idu,Idv exists in the trifocal tensor list + for (size_t i = 0; i < vec_triplets_validated.size(); ++i) + { + const graphUtils::Triplet & triplet = vec_triplets_validated[i]; + if ( triplet.contain(std::make_pair(Idu, Idv))) + { + edge_filter[iter] = true; + break; + } + } + } + + imageGraph::exportToGraphvizData( + stlplus::create_filespec(_sOutDirectory, "cleanedGraphTripletRotation"), + sg); + + { + std::cout << "\nTriplets filtering based on error on cycle \n"; + std::cout << "Before : " << vec_triplets.size() << " triplets \n" + << "After : " << vec_triplets_validated.size() << std::endl; + std::cout << "There is " << lemon::countConnectedComponents (sg) + << " Connected Component in the filtered graph" << std::endl; + } + + vec_triplets.clear(); + vec_triplets = vec_triplets_validated; + + size_t removedEdgesCount = 0; + + //-- Remove false edges from the rejected triplets + { + for (Graph::EdgeIt iter(putativeGraph.g); iter!=INVALID; ++iter) + { + if (!edge_filter[iter]) + { + removedEdgesCount++; + + size_t Idu = (*putativeGraph.map_nodeMapIndex)[sg.u(iter)]; + size_t Idv = (*putativeGraph.map_nodeMapIndex)[sg.v(iter)]; + + //-- Clean relatives matches + STLPairWiseMatches::iterator iterF = _map_Matches_F.find(std::make_pair(Idu,Idv)); + if (iterF != _map_Matches_F.end()) + { + _map_Matches_F.erase(iterF); + } + else + { + iterF = _map_Matches_F.find(std::make_pair(Idv,Idu)); + if (iterF != _map_Matches_F.end()) + _map_Matches_F.erase(iterF); + } + + //-- Clean relative motions + std::map< std::pair, std::pair >::iterator iterF2 = map_relatives.find(std::make_pair(Idu,Idv)); + if (iterF2 != map_relatives.end()) + { + map_relatives.erase(iterF2); + } + else + { + iterF2 = map_relatives.find(std::make_pair(Idv,Idu)); + if (iterF2!= map_relatives.end()) + map_relatives.erase(iterF2); + } + } + } + } + + std::cout << "\n Relatives edges removed by triplet checking : " << removedEdgesCount << std::endl; +} + +void GlobalReconstructionEngine::bundleAdjustment_t_Xi( + std::map & map_camera, + std::vector & vec_allScenes, + const STLMAPTracks & map_tracksSelected) +{ + using namespace std; + + const size_t nbCams = map_camera.size(); + const size_t nbPoints3D = vec_allScenes.size(); + + // Count the number of measurement (sum of the reconstructed track length) + size_t nbmeasurements = 0; + for (STLMAPTracks::const_iterator iterTracks = map_tracksSelected.begin(); + iterTracks != map_tracksSelected.end(); ++iterTracks) + { + const submapTrack & subTrack = iterTracks->second; + nbmeasurements += subTrack.size(); + } + + // Setup a BA problem + using namespace openMVG::bundle_adjustment; + BA_Problem_data<3> ba_problem; // Will refine translation and 3D points + + // Configure the size of the problem + ba_problem.num_cameras_ = nbCams; + ba_problem.num_points_ = nbPoints3D; + ba_problem.num_observations_ = nbmeasurements; + + ba_problem.point_index_.reserve(ba_problem.num_observations_); + ba_problem.camera_index_.reserve(ba_problem.num_observations_); + ba_problem.observations_.reserve(2 * ba_problem.num_observations_); + + ba_problem.num_parameters_ = 3 * ba_problem.num_cameras_ + 3 * ba_problem.num_points_; + ba_problem.parameters_.reserve(ba_problem.num_parameters_); + + // Fill camera + std::vector vec_Rot(map_camera.size()*3, 0.0); + size_t i = 0; + std::map map_camIndexToNumber; + for (std::map::const_iterator iter = map_camera.begin(); + iter != map_camera.end(); ++iter, ++i) + { + // in order to map camera index to contiguous number + map_camIndexToNumber.insert(std::make_pair(iter->first, i)); + + Mat3 R = iter->second._R; + double angleAxis[3]; + ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis); + vec_Rot[i*3] = angleAxis[0]; + vec_Rot[i*3+1] = angleAxis[1]; + vec_Rot[i*3+2] = angleAxis[2]; + + // translation + const Vec3 & t = iter->second._t; + ba_problem.parameters_.push_back(t[0]); + ba_problem.parameters_.push_back(t[1]); + ba_problem.parameters_.push_back(t[2]); + } + + // Fill 3D points + for (std::vector::const_iterator iter = vec_allScenes.begin(); + iter != vec_allScenes.end(); + ++iter) + { + const Vec3 & pt3D = *iter; + ba_problem.parameters_.push_back(pt3D[0]); + ba_problem.parameters_.push_back(pt3D[1]); + ba_problem.parameters_.push_back(pt3D[2]); + } + + // Fill the measurements + i = 0; + for (STLMAPTracks::const_iterator iterTracks = map_tracksSelected.begin(); + iterTracks != map_tracksSelected.end(); ++iterTracks, ++i) + { + const size_t trackId = iterTracks->first; + // Look through the track and add point position + const tracks::submapTrack & track = iterTracks->second; + + for( tracks::submapTrack::const_iterator iterTrack = track.begin(); + iterTrack != track.end(); + ++iterTrack) + { + size_t imageId = iterTrack->first; + size_t featId = iterTrack->second; + + // If imageId reconstructed : + // - Add measurements (the feature position) + // - Add camidx (map the image number to the camera index) + // - Add ptidx (the 3D corresponding point index) (must be increasing) + + //if ( set_camIndex.find(imageId) != set_camIndex.end()) + { + const std::vector & vec_feats = _map_feats[imageId]; + const SIOPointFeature & ptFeat = vec_feats[featId]; + const PinholeCamera & cam = map_camera[imageId]; + + double ppx = cam._K(0,2), ppy = cam._K(1,2); + ba_problem.observations_.push_back( ptFeat.x() - ppx ); + ba_problem.observations_.push_back( ptFeat.y() - ppy ); + + ba_problem.point_index_.push_back(i); + ba_problem.camera_index_.push_back(map_camIndexToNumber[imageId]); + } + } + } + + // The same K matrix is used by all the camera + const Mat3 _K = _vec_intrinsicGroups[0].m_K; + + // Create residuals for each observation in the bundle adjustment problem. The + // parameters for cameras and points are added automatically. + ceres::Problem problem; + for (size_t i = 0; i < ba_problem.num_observations(); ++i) { + // Each Residual block takes a point and a camera as input and outputs a 2 + // dimensional residual. Internally, the cost function stores the observed + // image location and compares the reprojection against the observation. + + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction( + new PinholeReprojectionError_t( + &ba_problem.observations()[2 * i + 0], + _K(0,0), + &vec_Rot[ba_problem.camera_index_[i]*3])); + + problem.AddResidualBlock(cost_function, + //NULL, // squared loss + new ceres::HuberLoss(Square(4.0)), + ba_problem.mutable_camera_for_observation(i), + ba_problem.mutable_point_for_observation(i)); + } + // Configure a BA engine and run it + // Make Ceres automatically detect the bundle structure. + ceres::Solver::Options options; + options.linear_solver_type = ceres::SPARSE_SCHUR; + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::SUITE_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; + else + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::CX_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::CX_SPARSE; + else + { + // No sparse backend for Ceres. + // Use dense solving + options.linear_solver_type = ceres::DENSE_SCHUR; + } + options.minimizer_progress_to_stdout = false; + options.logging_type = ceres::SILENT; +#ifdef USE_OPENMP + options.num_threads = omp_get_num_threads(); +#endif // USE_OPENMP + + // Solve BA + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + std::cout << summary.FullReport() << std::endl; + + // If no error, get back refined parameters + if (summary.termination_type != ceres::DID_NOT_RUN && + summary.termination_type != ceres::USER_ABORT && + summary.termination_type != ceres::NUMERICAL_FAILURE) + { + // Get back 3D points + i = 0; + for (std::vector::iterator iter = vec_allScenes.begin(); + iter != vec_allScenes.end(); ++iter, ++i) + { + const double * pt = ba_problem.mutable_points() + i*3; + Vec3 & pt3D = *iter; + pt3D = Vec3(pt[0], pt[1], pt[2]); + } + plyHelper::exportToPly(vec_allScenes, stlplus::create_filespec(_sOutDirectory, "raw_pointCloud_BA_T_Xi", "ply")); + + // Get back camera + i = 0; + for (std::map::iterator iter = map_camera.begin(); + iter != map_camera.end(); ++iter, ++i) + { + const double * cam = ba_problem.mutable_cameras() + i*3; + + Vec3 t(cam[0], cam[1], cam[2]); + PinholeCamera & sCam = iter->second; + Mat3 K = sCam._K; + Mat3 R = sCam._R; + sCam = PinholeCamera(K, R, t); + } + + { + //-- Export First bundle adjustment statistics + if (_bHtmlReport) + { + using namespace htmlDocument; + std::ostringstream os; + os << "First Bundle Adjustment (Ti, Xjs) statistics."; + _htmlDocStream->pushInfo("
"); + _htmlDocStream->pushInfo(htmlMarkup("h1",os.str())); + + os.str(""); + os << "-------------------------------" << "
" + << "-- #tracks: " << map_tracksSelected.size() << ".
" + << "-- #observation: " << ba_problem.num_observations_ << ".
" + << "-- initial residual mean (RMSE): " << std::sqrt(summary.initial_cost/ba_problem.num_observations_) << ".
" + << "-- residual mean (RMSE): " << std::sqrt(summary.final_cost/ba_problem.num_observations_) << ".
" + << "-- Nb Steps required until convergence : " << summary.num_successful_steps + summary.num_unsuccessful_steps << ".
" + << "-------------------------------" << "
"; + _htmlDocStream->pushInfo(os.str()); + } + } + } +} + +void GlobalReconstructionEngine::bundleAdjustment_Rt_Xi( + std::map & map_camera, + std::vector & vec_allScenes, + const STLMAPTracks & map_tracksSelected) +{ + using namespace std; + + const size_t nbCams = map_camera.size(); + const size_t nbPoints3D = vec_allScenes.size(); + + // Count the number of measurement (sum of the reconstructed track length) + size_t nbmeasurements = 0; + for (STLMAPTracks::const_iterator iterTracks = map_tracksSelected.begin(); + iterTracks != map_tracksSelected.end(); ++iterTracks) + { + const submapTrack & subTrack = iterTracks->second; + nbmeasurements += subTrack.size(); + } + + // Setup a BA problem + using namespace openMVG::bundle_adjustment; + BA_Problem_data<6> ba_problem; // Will refine translation and 3D points + + // Configure the size of the problem + ba_problem.num_cameras_ = nbCams; + ba_problem.num_points_ = nbPoints3D; + ba_problem.num_observations_ = nbmeasurements; + + ba_problem.point_index_.reserve(ba_problem.num_observations_); + ba_problem.camera_index_.reserve(ba_problem.num_observations_); + ba_problem.observations_.reserve(2 * ba_problem.num_observations_); + + ba_problem.num_parameters_ = 6 * ba_problem.num_cameras_ + 3 * ba_problem.num_points_; + ba_problem.parameters_.reserve(ba_problem.num_parameters_); + + // Fill camera + size_t i = 0; + std::map map_camIndexToNumber; + for (std::map::const_iterator iter = map_camera.begin(); + iter != map_camera.end(); ++iter, ++i) + { + // in order to map camera index to contiguous number + map_camIndexToNumber.insert(std::make_pair(iter->first, i)); + + const Mat3 & R = iter->second._R; + double angleAxis[3]; + ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis); + + // translation + const Vec3 & t = iter->second._t; + + ba_problem.parameters_.push_back(angleAxis[0]); + ba_problem.parameters_.push_back(angleAxis[1]); + ba_problem.parameters_.push_back(angleAxis[2]); + ba_problem.parameters_.push_back(t[0]); + ba_problem.parameters_.push_back(t[1]); + ba_problem.parameters_.push_back(t[2]); + } + + // Fill 3D points + for (std::vector::const_iterator iter = vec_allScenes.begin(); + iter != vec_allScenes.end(); + ++iter) + { + const Vec3 & pt3D = *iter; + ba_problem.parameters_.push_back(pt3D[0]); + ba_problem.parameters_.push_back(pt3D[1]); + ba_problem.parameters_.push_back(pt3D[2]); + } + + // Fill the measurements + i = 0; + for (STLMAPTracks::const_iterator iterTracks = map_tracksSelected.begin(); + iterTracks != map_tracksSelected.end(); ++iterTracks, ++i) + { + const size_t trackId = iterTracks->first; + // Look through the track and add point position + const tracks::submapTrack & track = iterTracks->second; + + for( tracks::submapTrack::const_iterator iterTrack = track.begin(); + iterTrack != track.end(); + ++iterTrack) + { + size_t imageId = iterTrack->first; + size_t featId = iterTrack->second; + + // If imageId reconstructed : + // - Add measurements (the feature position) + // - Add camidx (map the image number to the camera index) + // - Add ptidx (the 3D corresponding point index) (must be increasing) + + //if ( set_camIndex.find(imageId) != set_camIndex.end()) + { + const std::vector & vec_feats = _map_feats[imageId]; + const SIOPointFeature & ptFeat = vec_feats[featId]; + const PinholeCamera & cam = map_camera[imageId]; + + double ppx = cam._K(0,2), ppy = cam._K(1,2); + ba_problem.observations_.push_back( ptFeat.x() - ppx ); + ba_problem.observations_.push_back( ptFeat.y() - ppy ); + + ba_problem.point_index_.push_back(i); + ba_problem.camera_index_.push_back(map_camIndexToNumber[imageId]); + } + } + } + + // The same K matrix is used by all the camera + const Mat3 _K = _vec_intrinsicGroups[0].m_K; + + // Create residuals for each observation in the bundle adjustment problem. The + // parameters for cameras and points are added automatically. + ceres::Problem problem; + for (size_t k = 0; k < ba_problem.num_observations(); ++k) { + // Each Residual block takes a point and a camera as input and outputs a 2 + // dimensional residual. Internally, the cost function stores the observed + // image location and compares the reprojection against the observation. + + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction( + new PinholeReprojectionError_Rt( + &ba_problem.observations()[2 * k + 0], _K(0,0))); + + problem.AddResidualBlock(cost_function, + new ceres::HuberLoss(Square(2.0)), + //NULL, // squared loss + ba_problem.mutable_camera_for_observation(k), + ba_problem.mutable_point_for_observation(k)); + } + // Configure a BA engine and run it + // Make Ceres automatically detect the bundle structure. + ceres::Solver::Options options; + options.linear_solver_type = ceres::SPARSE_SCHUR; + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::SUITE_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; + else + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::CX_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::CX_SPARSE; + else + { + // No sparse backend for Ceres. + // Use dense solving + options.linear_solver_type = ceres::DENSE_SCHUR; + } + options.minimizer_progress_to_stdout = false; + options.logging_type = ceres::SILENT; +#ifdef USE_OPENMP + options.num_threads = omp_get_num_threads(); +#endif // USE_OPENMP + + // Solve BA + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + std::cout << summary.FullReport() << std::endl; + + // If no error, get back refined parameters + if (summary.termination_type != ceres::DID_NOT_RUN && + summary.termination_type != ceres::USER_ABORT && + summary.termination_type != ceres::NUMERICAL_FAILURE) + { + // Get back 3D points + i = 0; + for (std::vector::iterator iter = vec_allScenes.begin(); + iter != vec_allScenes.end(); ++iter, ++i) + { + const double * pt = ba_problem.mutable_points() + i*3; + Vec3 & pt3D = *iter; + pt3D = Vec3(pt[0], pt[1], pt[2]); + } + plyHelper::exportToPly(vec_allScenes, stlplus::create_filespec(_sOutDirectory, "raw_pointCloud_BA_RT_Xi", "ply")); + + // Get back camera + i = 0; + for (std::map::iterator iter = map_camera.begin(); + iter != map_camera.end(); ++iter, ++i) + { + const double * cam = ba_problem.mutable_cameras() + i*6; + Mat3 R; + // angle axis to rotation matrix + ceres::AngleAxisToRotationMatrix(cam, R.data()); + + Vec3 t(cam[3], cam[4], cam[5]); + double focal = cam[6]; + + // Update the camera + Mat3 K = iter->second._K; + PinholeCamera & sCam = iter->second; + sCam = PinholeCamera(K, R, t); + } + + { + //-- Export Second bundle adjustment statistics + if (_bHtmlReport) + { + using namespace htmlDocument; + std::ostringstream os; + os << "Second Bundle Adjustment (Ri, Ti, Xjs) statistics."; + _htmlDocStream->pushInfo("
"); + _htmlDocStream->pushInfo(htmlMarkup("h1",os.str())); + + os.str(""); + os << "-------------------------------" << "
" + << "-- #tracks: " << map_tracksSelected.size() << ".
" + << "-- #observation: " << ba_problem.num_observations_ << ".
" + << "-- residual mean (RMSE): " << std::sqrt(summary.final_cost/ba_problem.num_observations_) << ".
" + << "-- Nb Steps required until convergence : " << summary.num_successful_steps + summary.num_unsuccessful_steps << ".
" + << "-------------------------------" << "
"; + _htmlDocStream->pushInfo(os.str()); + } + + std::cout << "\n" + << "-------------------------------" << "\n" + << "-- #tracks: " << map_tracksSelected.size() << ".\n" + << "-- #observation: " << ba_problem.num_observations_ << ".\n" + << "-- residual mean (RMSE): " << std::sqrt(summary.final_cost/ba_problem.num_observations_) << ".\n" + << "-- Nb Steps required until convergence : " << summary.num_successful_steps + summary.num_unsuccessful_steps << ".\n" + << "-------------------------------" << std::endl; + + } + } +} + +} // namespace openMVG diff --git a/src/software/globalSfM/SfMGlobalEngine.hpp b/src/software/globalSfM/SfMGlobalEngine.hpp new file mode 100644 index 0000000000..a1d0c66cb5 --- /dev/null +++ b/src/software/globalSfM/SfMGlobalEngine.hpp @@ -0,0 +1,144 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_GLOBAL_SFM_ENGINE_H +#define OPENMVG_GLOBAL_SFM_ENGINE_H + +#include "openMVG/numeric/numeric.h" + +#include "openMVG/cameras/PinholeCamera.hpp" +#include "software/SfM/SfMEngine.hpp" +#include "software/SfM/SfMIOHelper.hpp" +class SIOPointFeature; + +#include "openMVG/tracks/tracks.hpp" +#include "openMVG/matching/indMatch.hpp" +using namespace openMVG::tracks; + +#include "openMVG/graph/triplet_finder.hpp" +#include "openMVG/linearProgramming/lInfinityCV/global_translations_fromTriplets.hpp" + +#include "third_party/htmlDoc/htmlDoc.hpp" + +#include + +namespace openMVG{ + +//------------------ +//-- Bibliography -- +//------------------ +//- [1] "Global Fusion of Relative Motions for Robust, Accurate and Scalable Structure from Motion." +//- Authors: Pierre MOULON, Pascal MONASSE and Renaud MARLET. +//- Date: December 2013. +//- Conference: ICCV. +//- [2] "Disambiguating visual relations using loop constraints." +//- Autors: Christopher ZACH, Manfred KLOPSCHITZ and Marc POLLEFEYS. +//- Date: 2010 +//- Conference : CVPR. + +// Implementation of [1] +// Some points differs from the [1] paper to ease open source port: +//-- Relative rotation inference: +// - only the triplet rejection is performed (in [1] a Bayesian inference on cycle error is performed [2]) +//-- Global rotation computation: +// - in [1] they are computed by a sparse least square formulation, here a simple dense least square is used. +//-- Linear Programming solver: +// - in order to have the best performance it is advised to used the MOSEK LP backend. + +class GlobalReconstructionEngine : public ReconstructionEngine +{ +public: + GlobalReconstructionEngine(const std::string & sImagePath, + const std::string & sMatchesPath, const std::string & sOutDirectory, + bool bHtmlReport = false); + + ~GlobalReconstructionEngine(); + + virtual bool Process(); + +private: + /// Read input data (point correspondences, K matrix) + bool ReadInputData(); + + bool CleanGraph(); + + void ComputeRelativeRt(std::map< std::pair, std::pair > & vec_relatives); + + // Detect and remove the outlier relative rotations + void rotationInference(std::map< std::pair, std::pair > & map_relatives); + + // Compute the global rotations + bool computeGlobalRotations( + const std::vector, Mat3> > & vec_relativeRotEstimate, + std::map & map_globalR) const; + + // List the triplet of the image connection graph (_map_Matches_F) + void tripletListing(std::vector< graphUtils::Triplet > & vec_triplets) const; + + // Relative rotations inference on relative rotations composition error along 3 length cycles (triplets). + void tripletRotationRejection( + std::vector< graphUtils::Triplet > & vec_triplets, + std::map< std::pair, std::pair > & map_relatives); + + // Compute relative translations over the graph of putative triplets + void computePutativeTranslation_EdgesCoverage( + const std::map & map_globalR, + const std::vector< graphUtils::Triplet > & vec_triplets, + std::vector & vec_initialEstimates, + tracks::mapPairWiseMatches & newpairMatches) const; + + // Bundle adjustment : refine structure Xis and camera translations + void bundleAdjustment_t_Xi( + std::map & map_camera, + std::vector & vec_allScenes, + const STLMAPTracks & map_tracksSelected); + + // Bundle adjustment : refine structure Xis and camera rotations and translations + void bundleAdjustment_Rt_Xi( + std::map & map_camera, + std::vector & vec_allScenes, + const STLMAPTracks & map_tracksSelected); + +private: + + // ----- + // Input data + // ---- + + // Image considered for the reconstruction + std::vector _vec_fileNames; + std::vector _vec_camImageNames; + std::vector _vec_intrinsicGroups; + std::map< size_t, std::vector > _map_feats; // feature per images + + typedef tracks::mapPairWiseMatches STLPairWiseMatches; + STLPairWiseMatches _map_Matches_F; // pairwise matches for Essential matrix model + + + //------ + //-- Mapping between camera node Ids and cameraIndex: + //-------------- + // Graph node Ids mapping to camera Ids + std::map map_cameraNodeToCameraIndex; // graph node Id to 0->Ncam + // camera Ids to graph node Ids + std::map map_cameraIndexTocameraNode; // 0->Ncam correspondance to graph node Id + //-- + //---- + + + // ----- + // Reporting .. + // ---- + bool _bHtmlReport; + std::auto_ptr _htmlDocStream; + +}; + + +} // namespace openMVG + +#endif // OPENMVG_GLOBAL_SFM_ENGINE_H diff --git a/src/software/globalSfM/SfMGlobalEngine_triplet_t_estimator.hpp b/src/software/globalSfM/SfMGlobalEngine_triplet_t_estimator.hpp new file mode 100644 index 0000000000..4a81498a6f --- /dev/null +++ b/src/software/globalSfM/SfMGlobalEngine_triplet_t_estimator.hpp @@ -0,0 +1,198 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_GLOBAL_SFM_ENGINE_TRIPLET_T_ESTIMATOR_H +#define OPENMVG_GLOBAL_SFM_ENGINE_TRIPLET_T_ESTIMATOR_H + +#include "openMVG/numeric/numeric.h" + +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#ifdef OPENMVG_HAVE_MOSEK +#include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" +#endif +#include "openMVG/linearProgramming/bisectionLP.hpp" +#include "openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri.hpp" + +#include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" + +namespace openMVG { +namespace trifocal { +namespace kernel { + +/// A trifocal tensor seen as 3 projective cameras +struct TrifocalTensorModel { + Mat34 P1, P2, P3; + + static double Error(const TrifocalTensorModel & t, const Vec2 & pt1, const Vec2 & pt2, const Vec2 & pt3) + { + // Triangulate and return the reprojection error + Triangulation triangulationObj; + triangulationObj.add(t.P1, pt1); + triangulationObj.add(t.P2, pt2); + triangulationObj.add(t.P3, pt3); + const Vec3 X = triangulationObj.compute(); + + //- Return max error as a test + double pt1ReProj = (Project(t.P1, X) - pt1).squaredNorm(); + double pt2ReProj = (Project(t.P2, X) - pt2).squaredNorm(); + double pt3ReProj = (Project(t.P3, X) - pt3).squaredNorm(); + + return std::max(pt1ReProj, std::max(pt2ReProj,pt3ReProj)); + } +}; + +} // namespace kernel +} // namespace trifocal +} // namespace openMVG + +namespace openMVG{ + +using namespace openMVG::trifocal::kernel; + +struct tisXisTrifocalSolver { + enum { MINIMUM_SAMPLES = 4 }; + enum { MAX_MODELS = 1 }; + // Solve the computation of the tensor. + static void Solve( + const Mat &pt0, const Mat & pt1, const Mat & pt2, + const std::vector & vec_KR, const Mat3 & K, std::vector *P) + { + //Build the megaMatMatrix + const int n_obs = pt0.cols(); + Mat4X megaMat(4, n_obs*3); + { + size_t cpt = 0; + for (size_t i = 0; i < n_obs; ++i) { + + megaMat.col(cpt) << pt0.col(i)(0), pt0.col(i)(1), i, 0; + ++cpt; + + megaMat.col(cpt) << pt1.col(i)(0), pt1.col(i)(1), i, 1; + ++cpt; + + megaMat.col(cpt) << pt2.col(i)(0), pt2.col(i)(1), i, 2; + ++cpt; + } + } + //-- Solve the LInfinity translation and structure from Rotation and points data. + std::vector vec_solution((3 + MINIMUM_SAMPLES)*3); + + using namespace openMVG::lInfinityCV; + +#ifdef OPENMVG_HAVE_MOSEK + MOSEK_SolveWrapper LPsolver(vec_solution.size()); +#else + OSI_CLP_SolverWrapper LPsolver(vec_solution.size()); +#endif + + Translation_Structure_L1_ConstraintBuilder cstBuilder(vec_KR, megaMat); + double gamma; + if (BisectionLP( + LPsolver, + cstBuilder, + &vec_solution, + .5/K(0,0),//admissibleResidual, + 0.0, 1e-8, 2, &gamma, false)) + { + std::vector vec_tis(3); + vec_tis[0] = Vec3(vec_solution[0], vec_solution[1], vec_solution[2]); + vec_tis[1] = Vec3(vec_solution[3], vec_solution[4], vec_solution[5]); + vec_tis[2] = Vec3(vec_solution[6], vec_solution[7], vec_solution[8]); + + TrifocalTensorModel PTemp; + PTemp.P1 = HStack(vec_KR[0], vec_tis[0]); + PTemp.P2 = HStack(vec_KR[1], vec_tis[1]); + PTemp.P3 = HStack(vec_KR[2], vec_tis[2]); + + P->push_back(PTemp); + } + } + + // Compute the residual of reprojections + static double Error(const TrifocalTensorModel & Tensor, const Vec2 & pt0, const Vec2 & pt1, const Vec2 & pt2) + { + return TrifocalTensorModel::Error(Tensor, pt0, pt1, pt2); + } +}; + +template +class TrifocalKernel_ACRansac_N_tisXis +{ +public: + typedef SolverArg Solver; + typedef ModelArg Model; + + + TrifocalKernel_ACRansac_N_tisXis(const Mat & x1, const Mat & x2, const Mat & x3, + int w, int h, const std::vector & vec_KRi, const Mat3 & K) + : x1_(x1), x2_(x2), x3_(x3), logalpha0_(0.0), vec_KR_(vec_KRi), K_(K) + { + // Normalize points by inverse(K) + + N_ = K_.inverse(); + ApplyTransformationToPoints(x1_, N_, &x1n_); + ApplyTransformationToPoints(x2_, N_, &x2n_); + ApplyTransformationToPoints(x3_, N_, &x3n_); + + vec_KR_[0] = N_ * vec_KR_[0]; + vec_KR_[1] = N_ * vec_KR_[1]; + vec_KR_[2] = N_ * vec_KR_[2]; + + logalpha0_ = log10(M_PI); + } + + enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES }; + enum { MAX_MODELS = Solver::MAX_MODELS }; + + void Fit(const std::vector &samples, std::vector *models) const { + + // Create a model from the points + Solver::Solve( + ExtractColumns(x1n_, samples), + ExtractColumns(x2n_, samples), + ExtractColumns(x3n_, samples), + vec_KR_, K_, models); + } + + double Error(size_t sample, const Model &model) const { + return ErrorArg::Error(model, x1n_.col(sample), x2n_.col(sample), x3n_.col(sample)); + } + + size_t NumSamples() const { + return x1n_.cols(); + } + + void Unnormalize(Model * model) const { + // Unnormalize model from the computed conditioning. + model->P1 = K_ * model->P1; + model->P2 = K_ * model->P2; + model->P3 = K_ * model->P3; + } + + double logalpha0() const {return logalpha0_;} + + double multError() const {return 1.0;} + + Mat3 normalizer1() const {return N_;} + Mat3 normalizer2() const {return Mat3::Identity();} + double unormalizeError(double val) const { return sqrt(val) / N_(0,0);} + +private: + const Mat & x1_, & x2_, & x3_; + Mat x1n_, x2n_, x3n_; + Mat3 N_; + double logalpha0_; + std::vector vec_KR_; + Mat3 K_; +}; + +} // namespace openMVG + +#endif // OPENMVG_GLOBAL_SFM_ENGINE_TRIPLET_T_ESTIMATOR_H diff --git a/src/software/globalSfM/SfMGlobal_tij_computation.hpp b/src/software/globalSfM/SfMGlobal_tij_computation.hpp new file mode 100644 index 0000000000..9d2822903c --- /dev/null +++ b/src/software/globalSfM/SfMGlobal_tij_computation.hpp @@ -0,0 +1,537 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_GLOBAL_SFM_ENGINE_TIJ_COMPUTATION_H +#define OPENMVG_GLOBAL_SFM_ENGINE_TIJ_COMPUTATION_H + +#include "openMVG/numeric/numeric.h" +#include "openMVG/tracks/tracks.hpp" +#include "software/globalSfM/SfMGlobalEngine.hpp" +#include "software/globalSfM/SfMGlobalEngine_triplet_t_estimator.hpp" + +#undef DYNAMIC +#include "openMVG/bundle_adjustment/problem_data_container.hpp" +#include "software/globalSfM/SfMBundleAdjustmentHelper_tonly.hpp" + +#include "openMVG/matching/indexed_sort.hpp" + +#include "software/globalSfM/mutexSet.hpp" + +namespace openMVG{ + +// Robust estimation and refinement of a translation and 3D points of an image triplets. +bool estimate_T_triplet( + size_t w, size_t h, + const openMVG::tracks::STLMAPTracks & map_tracksCommon, + const std::map > & map_feats, + const std::vector & vec_global_KR_Triplet, + const Mat3 & K, + std::vector & vec_tis, + double & dPrecision, + std::vector & vec_inliers) +{ + using namespace linearProgramming; + using namespace lInfinityCV; + + // Convert data + Mat x1(2, map_tracksCommon.size()); + Mat x2(2, map_tracksCommon.size()); + Mat x3(2, map_tracksCommon.size()); + + Mat* xxx[3] = {&x1, &x2, &x3}; + + size_t cpt = 0; + for (STLMAPTracks::const_iterator iterTracks = map_tracksCommon.begin(); + iterTracks != map_tracksCommon.end(); ++iterTracks, ++cpt) { + const submapTrack & subTrack = iterTracks->second; + size_t index = 0; + for (submapTrack::const_iterator iter = subTrack.begin(); iter != subTrack.end(); ++iter, ++index) { + const size_t imaIndex = iter->first; + const size_t featIndex = iter->second; + const SIOPointFeature & pt = map_feats.find(imaIndex)->second[featIndex]; + xxx[index]->col(cpt)(0) = pt.x(); + xxx[index]->col(cpt)(1) = pt.y(); + } + } + + using namespace openMVG::trifocal; + using namespace openMVG::trifocal::kernel; + + typedef TrifocalKernel_ACRansac_N_tisXis< + tisXisTrifocalSolver, + tisXisTrifocalSolver, + TrifocalTensorModel> KernelType; + KernelType kernel(x1, x2, x3, w, h, vec_global_KR_Triplet, K); + + const size_t ORSA_ITER = 320; + + TrifocalTensorModel T; + Mat3 Kinv = K.inverse(); + dPrecision = dPrecision * Kinv(0,0) * Kinv(0,0);//std::numeric_limits::infinity(); + std::pair acStat = ACRANSAC(kernel, vec_inliers, ORSA_ITER, &T, dPrecision, false); + dPrecision = acStat.first; + + //-- Export data in order to have an idea of the precision of the estimates + vec_tis.resize(3); + Mat3 K2, R; + Vec3 t; + KRt_From_P(T.P1, &K2, &R, &t); + vec_tis[0] = t; + KRt_From_P(T.P2, &K2, &R, &t); + vec_tis[1] = t; + KRt_From_P(T.P3, &K2, &R, &t); + vec_tis[2] = t; + + // Fill Xis + std::vector vec_residuals(vec_inliers.size()); + std::vector vec_Xis(vec_inliers.size()); + for (size_t i = 0; i < vec_inliers.size(); ++i) { + + Triangulation triangulation; + triangulation.add(T.P1, x1.col(vec_inliers[i])); + triangulation.add(T.P2, x2.col(vec_inliers[i])); + triangulation.add(T.P3, x3.col(vec_inliers[i])); + vec_residuals[i] = triangulation.error(); + vec_Xis[i] = triangulation.compute(); + } + + double min, max, mean, median; + minMaxMeanMedian(vec_residuals.begin(), vec_residuals.end(), + min, max, mean, median); + + bool bTest(vec_inliers.size() > 30); + + if (!bTest) + { + std::cout << "Triplet rejected : AC: " << dPrecision + << " median: " << median + << " inliers count " << vec_inliers.size() + << " total putative " << map_tracksCommon.size() << std::endl; + } + + bool bRefine = true; + if (bRefine && bTest) + { + // BA on tis, Xis + + const size_t nbCams = 3; + const size_t nbPoints3D = vec_Xis.size(); + + // Count the number of measurement (sum of the reconstructed track length) + size_t nbmeasurements = nbPoints3D*3; + + // Setup a BA problem + using namespace openMVG::bundle_adjustment; + BA_Problem_data<3> ba_problem; // Will refine translation and 3D points + + // Configure the size of the problem + ba_problem.num_cameras_ = nbCams; + ba_problem.num_points_ = nbPoints3D; + ba_problem.num_observations_ = nbmeasurements; + + ba_problem.point_index_.reserve(ba_problem.num_observations_); + ba_problem.camera_index_.reserve(ba_problem.num_observations_); + ba_problem.observations_.reserve(2 * ba_problem.num_observations_); + + ba_problem.num_parameters_ = 3 * ba_problem.num_cameras_ + 3 * ba_problem.num_points_; + ba_problem.parameters_.reserve(ba_problem.num_parameters_); + + // Fill camera + std::vector vec_Rot(vec_Xis.size()*3, 0.0); + { + Mat3 R = K.inverse() * vec_global_KR_Triplet[0]; + double angleAxis[3]; + ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis); + vec_Rot[0] = angleAxis[0]; + vec_Rot[1] = angleAxis[1]; + vec_Rot[2] = angleAxis[2]; + + // translation + ba_problem.parameters_.push_back(vec_tis[0](0)); + ba_problem.parameters_.push_back(vec_tis[0](1)); + ba_problem.parameters_.push_back(vec_tis[0](2)); + } + { + Mat3 R = K.inverse() * vec_global_KR_Triplet[1]; + double angleAxis[3]; + ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis); + vec_Rot[3] = angleAxis[0]; + vec_Rot[4] = angleAxis[1]; + vec_Rot[5] = angleAxis[2]; + + // translation + ba_problem.parameters_.push_back(vec_tis[1](0)); + ba_problem.parameters_.push_back(vec_tis[1](1)); + ba_problem.parameters_.push_back(vec_tis[1](2)); + } + { + Mat3 R = K.inverse() * vec_global_KR_Triplet[2]; + double angleAxis[3]; + ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis); + vec_Rot[6] = angleAxis[0]; + vec_Rot[7] = angleAxis[1]; + vec_Rot[8] = angleAxis[2]; + + // translation + ba_problem.parameters_.push_back(vec_tis[2](0)); + ba_problem.parameters_.push_back(vec_tis[2](1)); + ba_problem.parameters_.push_back(vec_tis[2](2)); + } + + // Fill 3D points + for (std::vector::const_iterator iter = vec_Xis.begin(); + iter != vec_Xis.end(); + ++iter) + { + const Vec3 & pt3D = *iter; + ba_problem.parameters_.push_back(pt3D[0]); + ba_problem.parameters_.push_back(pt3D[1]); + ba_problem.parameters_.push_back(pt3D[2]); + } + + // Fill the measurements + for (size_t i = 0; i < vec_inliers.size(); ++i) + { + double ppx = K(0,2), ppy = K(1,2); + Vec2 ptFeat = x1.col(vec_inliers[i]); + ba_problem.observations_.push_back( ptFeat.x() - ppx ); + ba_problem.observations_.push_back( ptFeat.y() - ppy ); + + ba_problem.point_index_.push_back(i); + ba_problem.camera_index_.push_back(0); + + ptFeat = x2.col(vec_inliers[i]); + ba_problem.observations_.push_back( ptFeat.x() - ppx ); + ba_problem.observations_.push_back( ptFeat.y() - ppy ); + + ba_problem.point_index_.push_back(i); + ba_problem.camera_index_.push_back(1); + + ptFeat = x3.col(vec_inliers[i]); + ba_problem.observations_.push_back( ptFeat.x() - ppx ); + ba_problem.observations_.push_back( ptFeat.y() - ppy ); + + ba_problem.point_index_.push_back(i); + ba_problem.camera_index_.push_back(2); + } + + // Create residuals for each observation in the bundle adjustment problem. The + // parameters for cameras and points are added automatically. + ceres::Problem problem; + for (size_t i = 0; i < ba_problem.num_observations(); ++i) { + // Each Residual block takes a point and a camera as input and outputs a 2 + // dimensional residual. Internally, the cost function stores the observed + // image location and compares the reprojection against the observation. + + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction( + new PinholeReprojectionError_t( + &ba_problem.observations()[2 * i + 0], + K(0,0), + &vec_Rot[ba_problem.camera_index_[i]*3])); + + problem.AddResidualBlock(cost_function, + NULL, // squared loss + //new ceres::HuberLoss(Square(4.0)), + ba_problem.mutable_camera_for_observation(i), + ba_problem.mutable_point_for_observation(i)); + } + // Configure a BA engine and run it + // Make Ceres automatically detect the bundle structure. + ceres::Solver::Options options; + options.linear_solver_type = ceres::SPARSE_SCHUR; + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::SUITE_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; + else + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::CX_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::CX_SPARSE; + else + { + // No sparse backend for Ceres. + // Use dense solving + options.linear_solver_type = ceres::DENSE_SCHUR; + } + + options.minimizer_progress_to_stdout = false; + options.logging_type = ceres::SILENT; +#ifdef USE_OPENMP + options.num_threads = omp_get_num_threads(); +#endif // USE_OPENMP + + // Solve BA + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // If convergence and no error, get back refined parameters + if (summary.termination_type != ceres::DID_NOT_RUN && + summary.termination_type != ceres::USER_ABORT && + summary.termination_type != ceres::NUMERICAL_FAILURE) + { + size_t i = 0; + // Get back updated cameras + Vec3 * tt[3] = {&vec_tis[0], &vec_tis[1], &vec_tis[2]}; + for (i=0; i < 3; ++i) + { + const double * cam = ba_problem.mutable_cameras() + i*3; + + (*tt[i]) = Vec3(cam[0], cam[1], cam[2]); + } + } + } + return bTest; +} + +//-- Perform a trifocal estimation of the graph contain in vec_triplets with an +// edge coverage algorithm. It's complexity is sub-linear in term of edges count. +void GlobalReconstructionEngine::computePutativeTranslation_EdgesCoverage( + const std::map & map_globalR, + const std::vector< graphUtils::Triplet > & vec_triplets, + std::vector & vec_initialEstimates, + tracks::mapPairWiseMatches & newpairMatches) const +{ + // The same K matrix is used by all the camera + const Mat3 _K = _vec_intrinsicGroups[0].m_K; + + //-- Prepare global rotations + std::map map_global_KR; + for (std::map::const_iterator iter = map_globalR.begin(); + iter != map_globalR.end(); ++iter) + { + map_global_KR[iter->first] = _K * iter->second; + } + + //-- Prepare tracks count per triplets: + std::map map_tracksPerTriplets; + for (size_t i = 0; i < vec_triplets.size(); ++i) + { + const graphUtils::Triplet & triplet = vec_triplets[i]; + const size_t I = triplet.i, J = triplet.j , K = triplet.k; + + STLPairWiseMatches map_matchesIJK; + if(_map_Matches_F.find(std::make_pair(I,J)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(I,J))); + else + if(_map_Matches_F.find(std::make_pair(J,I)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(J,I))); + + if(_map_Matches_F.find(std::make_pair(I,K)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(I,K))); + else + if(_map_Matches_F.find(std::make_pair(K,I)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(K,I))); + + if(_map_Matches_F.find(std::make_pair(J,K)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(J,K))); + else + if(_map_Matches_F.find(std::make_pair(K,J)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(K,J))); + + // Compute tracks: + openMVG::tracks::STLMAPTracks map_tracks; + TracksBuilder tracksBuilder; + { + tracksBuilder.Build(map_matchesIJK); + tracksBuilder.Filter(3); + tracksBuilder.ExportToSTL(map_tracks); + } + map_tracksPerTriplets[i] = map_tracks.size(); + } + + typedef std::pair myEdge; + + //-- List all edges + std::set set_edges; + + for (size_t i = 0; i < vec_triplets.size(); ++i) + { + const graphUtils::Triplet & triplet = vec_triplets[i]; + size_t I = triplet.i, J = triplet.j , K = triplet.k; + // Add three edges + set_edges.insert(std::make_pair(std::min(I,J), std::max(I,J))); + set_edges.insert(std::make_pair(std::min(I,K), std::max(I,K))); + set_edges.insert(std::make_pair(std::min(J,K), std::max(J,K))); + } + + // Copy them in vector in order to try to compute them in parallel + std::vector vec_edges(set_edges.begin(), set_edges.end()); + + MutexSet m_mutexSet; + + std::cout << std::endl + << "Computation of the relative translations over the graph with an edge coverage algorithm" << std::endl; +#ifdef USE_OPENMP +//#pragma omp parallel for schedule(dynamic) +#pragma omp parallel for schedule(static, 6) +#endif + for (int k = 0; k < vec_edges.size(); ++k) + { + const myEdge & edge = vec_edges[k]; + //-- If current edge already computed continue + if (m_mutexSet.isDiscarded(edge) || m_mutexSet.size() == vec_edges.size()) + { + std::cout << "EDGES WAS PREVIOUSLY COMPUTED" << std::endl; + continue; + } + + std::vector vec_possibleTriplets; + // Find the triplet that contain the given edge + for (size_t i = 0; i < vec_triplets.size(); ++i) + { + const graphUtils::Triplet & triplet = vec_triplets[i]; + if (triplet.contain(edge)) + { + vec_possibleTriplets.push_back(i); + } + } + + //-- Sort the triplet according the number of matches they have on their edges + std::vector vec_commonTracksPerTriplets; + for (size_t i = 0; i < vec_possibleTriplets.size(); ++i) + { + const graphUtils::Triplet & triplet = vec_triplets[vec_possibleTriplets[i]]; + vec_commonTracksPerTriplets.push_back(map_tracksPerTriplets[vec_possibleTriplets[i]]); + } + //-- If current edge already computed continue + if (m_mutexSet.isDiscarded(edge)) + continue; + + using namespace indexed_sort; + std::vector< sort_index_packet_descend < size_t, size_t> > packet_vec(vec_commonTracksPerTriplets.size()); + sort_index_helper(packet_vec, &vec_commonTracksPerTriplets[0]); + + std::vector vec_possibleTripletsSorted; + for (int i = 0; i < vec_commonTracksPerTriplets.size(); ++i) { + vec_possibleTripletsSorted.push_back( vec_possibleTriplets[packet_vec[i].index] ); + } + vec_possibleTriplets = vec_possibleTripletsSorted; + + // Try to solve the triplet + // Search the possible triplet: + for (size_t i = 0; i < vec_possibleTriplets.size(); ++i) + { + const graphUtils::Triplet & triplet = vec_triplets[vec_possibleTriplets[i]]; + size_t I = triplet.i, J = triplet.j , K = triplet.k; + { + STLPairWiseMatches map_matchesIJK; + if(_map_Matches_F.find(std::make_pair(I,J)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(I,J))); + else + if(_map_Matches_F.find(std::make_pair(J,I)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(J,I))); + + if(_map_Matches_F.find(std::make_pair(I,K)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(I,K))); + else + if(_map_Matches_F.find(std::make_pair(K,I)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(K,I))); + + if(_map_Matches_F.find(std::make_pair(J,K)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(J,K))); + else + if(_map_Matches_F.find(std::make_pair(K,J)) != _map_Matches_F.end()) + map_matchesIJK.insert(*_map_Matches_F.find(std::make_pair(K,J))); + + // Select common point: + STLMAPTracks map_tracksCommon; + TracksBuilder tracksBuilder; + { + tracksBuilder.Build(map_matchesIJK); + tracksBuilder.Filter(3); + tracksBuilder.ExportToSTL(map_tracksCommon); + } + + // Try to estimate this triplets: + size_t w = _vec_intrinsicGroups[_vec_camImageNames[I].m_intrinsicId].m_w; + size_t h = _vec_intrinsicGroups[_vec_camImageNames[I].m_intrinsicId].m_h; + + std::vector vec_tis(3); + + // Get rotation: + std::vector vec_global_KR_Triplet; + vec_global_KR_Triplet.push_back(map_global_KR[I]); + vec_global_KR_Triplet.push_back(map_global_KR[J]); + vec_global_KR_Triplet.push_back(map_global_KR[K]); + + double dPrecision = 4.0; + + std::vector vec_inliers; + + if (map_tracksCommon.size() > 50 && + estimate_T_triplet( + w, h, + map_tracksCommon, _map_feats, vec_global_KR_Triplet, _K, + vec_tis, dPrecision, vec_inliers)) + { + std::cout << dPrecision << "\t" << vec_inliers.size() << std::endl; + + //-- Build the three camera: + const Mat3 RI = map_globalR.find(I)->second; + const Mat3 RJ = map_globalR.find(J)->second; + const Mat3 RK = map_globalR.find(K)->second; + const Vec3 ti = vec_tis[0]; + const Vec3 tj = vec_tis[1]; + const Vec3 tk = vec_tis[2]; + + // Build the 3 relative translations estimations. + // IJ, JK, IK + +//--- ATOMIC +#ifdef USE_OPENMP + #pragma omp critical +#endif + { + Mat3 RijGt; + Vec3 tij; + RelativeCameraMotion(RI, ti, RJ, tj, &RijGt, &tij); + vec_initialEstimates.push_back( + std::make_pair(std::make_pair(I, J), std::make_pair(RijGt, tij))); + + Mat3 RjkGt; + Vec3 tjk; + RelativeCameraMotion(RJ, tj, RK, tk, &RjkGt, &tjk); + vec_initialEstimates.push_back( + std::make_pair(std::make_pair(J, K), std::make_pair(RjkGt, tjk))); + + Mat3 RikGt; + Vec3 tik; + RelativeCameraMotion(RI, ti, RK, tk, &RikGt, &tik); + vec_initialEstimates.push_back( + std::make_pair(std::make_pair(I, K), std::make_pair(RikGt, tik))); + + // Add trifocal inliers as valid 3D points + for (std::vector::const_iterator iterInliers = vec_inliers.begin(); + iterInliers != vec_inliers.end(); ++iterInliers) + { + STLMAPTracks::const_iterator iterTracks = map_tracksCommon.begin(); + std::advance(iterTracks, *iterInliers); + const submapTrack & subTrack = iterTracks->second; + size_t iTriplet = 0; + submapTrack::const_iterator iterI, iterJ, iterK; + iterI = iterJ = iterK = subTrack.begin(); + std::advance(iterJ,1); + std::advance(iterK,2); + + newpairMatches[std::make_pair(I,J)].push_back(IndMatch(iterI->second, iterJ->second)); + newpairMatches[std::make_pair(J,K)].push_back(IndMatch(iterJ->second, iterK->second)); + newpairMatches[std::make_pair(I,K)].push_back(IndMatch(iterI->second, iterK->second)); + } + } + + //-- Remove the 3 edges validated by the trifocal tensor + m_mutexSet.discard(std::make_pair(std::min(I,J), std::max(I,J))); + m_mutexSet.discard(std::make_pair(std::min(I,K), std::max(I,K))); + m_mutexSet.discard(std::make_pair(std::min(J,K), std::max(J,K))); + break; + } + } + } + } +} + +} // namespace openMVG + +#endif // OPENMVG_GLOBAL_SFM_ENGINE_TIJ_COMPUTATION_H diff --git a/src/software/globalSfM/indexedImageGraph.hpp b/src/software/globalSfM/indexedImageGraph.hpp new file mode 100644 index 0000000000..772505fe90 --- /dev/null +++ b/src/software/globalSfM/indexedImageGraph.hpp @@ -0,0 +1,92 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_INDEXED_IMAGE_GRAPH_H_ +#define OPENMVG_INDEXED_IMAGE_GRAPH_H_ + +#include +#include +#include +#include +#include + +#include "lemon/list_graph.h" +using namespace lemon; +#include "openMVG/matching/indMatch.hpp" +using namespace openMVG::matching; + +namespace openMVG { +namespace imageGraph { + using namespace std; + +// Structure used to keep information of an image graph : +// - A graph (connection between nodes +// - Node => linked to String (the name of the image) +// - EdgeMap => Number of point connection between the source and target +// +struct indexedImageGraph +{ + typedef lemon::ListGraph GraphT; + typedef std::map map_Size_t_Node; + typedef GraphT::NodeMap map_NodeMapIndex; + typedef GraphT::NodeMap map_NodeMapName; + typedef GraphT::EdgeMap map_EdgeMap; + + GraphT g; + map_Size_t_Node map_size_t_to_node; // Original image index to graph node + auto_ptr map_nodeMapIndex; // Association of data to graph Node + auto_ptr map_codeMapName; // Association of data to graph Node + auto_ptr map_edgeMap; // Number of point matches between the source and the target + + indexedImageGraph( const map< std::pair, std::vector > & map_indexedMatches, + const std::vector &vec_fileNames) + { + map_nodeMapIndex = auto_ptr( new map_NodeMapIndex(g) ); + map_codeMapName = auto_ptr( new map_NodeMapName(g) ); + map_edgeMap = auto_ptr( new map_EdgeMap(g) ); + + //A-- Compute the number of node we need + set setNodes; + for (map< std::pair, vector >::const_iterator iter = map_indexedMatches.begin(); + iter != map_indexedMatches.end(); + ++iter) + { + setNodes.insert(iter->first.first); + setNodes.insert(iter->first.second); + } + + //B-- Create a node graph for each element of the set + for (set::const_iterator iter = setNodes.begin(); + iter != setNodes.end(); + ++iter) + { + map_size_t_to_node[*iter] = g.addNode(); + (*map_nodeMapIndex) [map_size_t_to_node[*iter]] = *iter; + (*map_codeMapName) [map_size_t_to_node[*iter]] = vec_fileNames[*iter]; + } + + //C-- Add weighted edges from the "map_indexedMatches" object + for (map< std::pair, vector >::const_iterator iter = map_indexedMatches.begin(); + iter != map_indexedMatches.end(); + ++iter) + { + const std::vector & vec_FilteredMatches = iter->second; + if (vec_FilteredMatches.size() > 0) + { + const size_t i = iter->first.first; + const size_t j = iter->first.second; + GraphT::Edge edge = g.addEdge(map_size_t_to_node[i], map_size_t_to_node[j]); + (*map_edgeMap)[ edge ] = vec_FilteredMatches.size(); + } + } + } +}; + +} // namespace imageGraph +} // namespace openMVG + +#endif // OPENMVG_INDEXED_IMAGE_GRAPH_H_ diff --git a/src/software/globalSfM/indexedImageGraphExport.hpp b/src/software/globalSfM/indexedImageGraphExport.hpp new file mode 100644 index 0000000000..8ee06e4713 --- /dev/null +++ b/src/software/globalSfM/indexedImageGraphExport.hpp @@ -0,0 +1,145 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_INDEXED_IMAGE_GRAPH_EXPORT_H_ +#define OPENMVG_INDEXED_IMAGE_GRAPH_EXPORT_H_ + +#include + +#include +#include + +namespace openMVG { +namespace imageGraph { + using namespace std; + +// Export an Image connection graph +// to graphviz file format. +// Example : +// graph 1 { +// node [shape=circle] +// n2 +// n1 +// n0 +// n1 -- n0 +// n2 -- n0 +// n2 -- n1 +// } +template +bool exportToGraphvizFormat_Nodal( + const GraphT & g, + ostream & os) +{ + os << "graph 1 {" << endl; + os << "node [shape=circle]" << endl; + //Export node label + for(typename GraphT::NodeIt n(g); n!= lemon::INVALID; ++n) + { + os << " n" << g.id(n) << std::endl; + } + + //-- Export arc (as the graph is bi-directional, export arc only one time) + + map< std::pair, size_t > map_arcs; + for(typename GraphT::ArcIt e(g); e!=lemon::INVALID; ++e) { + if( map_arcs.end() == map_arcs.find(std::make_pair(size_t (g.id(g.source(e))), size_t (g.id(g.target(e))))) + && + map_arcs.end() == map_arcs.find(std::make_pair(size_t (g.id(g.target(e))), size_t (g.id(g.source(e)))))) + { + map_arcs[std::pair(size_t (g.id(g.source(e))), + size_t (g.id(g.target(e)))) ] = 1.0; + } + } + //os << "edge [style=bold]" << endl; + for ( map< std::pair, size_t >::const_iterator iter = map_arcs.begin(); + iter != map_arcs.end(); + ++iter) + { + os << " n" << iter->first.first << " -- " << " n" << iter->first.second << endl; + } + + os << "}" << endl; + return os.good(); +} + +// Export the Image connection graph +// to the graphviz file format. +// Add the image name and preview in the graph by using HTML label. +template +bool exportToGraphvizFormat_Image( + const GraphT & g, + const NodeMap & nodeMap, + const EdgeMap & edgeMap, + ostream & os, bool bWeightedEdge=false) +{ + os << "graph 1 {" << endl; + os << "node [shape=none]" << endl; + //Export node label + for(typename GraphT::NodeIt n(g); n!=lemon::INVALID; ++n) + { + os << " n" << g.id(n) + << "[ label =" + << + "< "<< endl + <<""<< endl + <<""<< endl + <<""<< endl + <<"
" << "\"" << nodeMap[n] <<"\"" <<"
"<< endl + <<">, cluster=1];"<< endl; + + //os << " n" << g.id(n) + // << " [ " + // << " image=\"" << nodeMap[n] << "\" cluster=1]; " << endl; + } + + //Export arc value + map< std::pair, size_t > map_arcs; + for(typename GraphT::ArcIt e(g); e!=lemon::INVALID; ++e) { + if( map_arcs.end() == map_arcs.find(std::make_pair(size_t (g.id(g.source(e))), size_t (g.id(g.target(e))))) + && + map_arcs.end() == map_arcs.find(std::make_pair(size_t (g.id(g.target(e))), size_t (g.id(g.source(e)))))) + { + map_arcs[std::pair(size_t (g.id(g.source(e))), + size_t (g.id(g.target(e)))) ] = edgeMap[e]; + } + } + + os << "edge [style=bold]" << endl; + for ( map< std::pair, size_t >::const_iterator iter = map_arcs.begin(); + iter != map_arcs.end(); + ++iter) + { + if (bWeightedEdge) + { + os << " n" << iter->first.first << " -- " << " n" << iter->first.second + << " [label=\"" << iter->second << "\"]" << endl; + } + else + { + os << " n" << iter->first.first << " -- " << " n" << iter->first.second << endl; + } + } + os << "}" << endl; + return os.good(); +} + +template +void exportToGraphvizData(const std::string& sfile, const GraphT & graph){ + //Prepare Data + + std::ofstream file(sfile.c_str()); + openMVG::imageGraph::exportToGraphvizFormat_Nodal(graph, file); + file.close(); + + //Use Graphviz + const std::string cmd = "neato -Tsvg -O -Goverlap=scale -Gsplines=false " + sfile; + const int value = system(cmd.c_str()); +} + +} // namespace imageGraph +} // namespace openMVG +#endif // OPENMVG_INDEXED_IMAGE_GRAPH_EXPORT_H_ diff --git a/src/software/globalSfM/main_GlobalSfM.cpp b/src/software/globalSfM/main_GlobalSfM.cpp new file mode 100644 index 0000000000..3562555d9d --- /dev/null +++ b/src/software/globalSfM/main_GlobalSfM.cpp @@ -0,0 +1,75 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#include + +#include "software/globalSfM/SfMGlobalEngine.hpp" +using namespace openMVG; + +#include "third_party/cmdLine/cmdLine.h" +#include "third_party/stlplus3/filesystemSimplified/file_system.hpp" + +int main(int argc, char **argv) +{ + using namespace std; + std::cout << "Global Structure from Motion :" + << " open Source implementation of: \n" + << "\"Global Fusion of Relative Motions for Robust, Accurate and Scalable Structure from Motion.\"" + << " ICCV 2013 paper. Pierre Moulon, Pascal Monasse and Renaud Marlet." << std::endl + << std::endl; + + CmdLine cmd; + + std::string sImaDirectory; + std::string sMatchesDir; + std::string sOutDir = ""; + + cmd.add( make_option('i', sImaDirectory, "imadir") ); + cmd.add( make_option('m', sMatchesDir, "matchdir") ); + cmd.add( make_option('o', sOutDir, "outdir") ); + + try { + if (argc == 1) throw std::string("Invalid parameter."); + cmd.process(argc, argv); + } catch(const std::string& s) { + std::cerr << "Usage: " << argv[0] << ' ' + << "[-i|--imadir path] " + << "[-m|--matchdir path] " + << "[-o|--outdir path] " + << std::endl; + + std::cerr << s << std::endl; + return EXIT_FAILURE; + } + + if (sOutDir.empty()) { + std::cerr << "\nIt is an invalid output directory" << std::endl; + return EXIT_FAILURE; + } + + if (!stlplus::folder_exists(sOutDir)) + stlplus::folder_create(sOutDir); + + //--------------------------------------- + // Incremental reconstruction process + //--------------------------------------- + + clock_t timeStart = clock(); + GlobalReconstructionEngine to3DEngine(sImaDirectory, + sMatchesDir, + sOutDir, + true); + + if (to3DEngine.Process()) + { + clock_t timeEnd = clock(); + std::cout << std::endl << " Total Ac-Global-Sfm took : " << (timeEnd - timeStart) / CLOCKS_PER_SEC << std::endl; + return EXIT_SUCCESS; + } + + return EXIT_FAILURE; +} diff --git a/src/software/globalSfM/main_computeMatchesEssential.cpp b/src/software/globalSfM/main_computeMatchesEssential.cpp new file mode 100644 index 0000000000..9c71ce99ae --- /dev/null +++ b/src/software/globalSfM/main_computeMatchesEssential.cpp @@ -0,0 +1,329 @@ + +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + + +#include "openMVG/image/image.hpp" +#include "openMVG/features/features.hpp" + +/// Generic Image Collection image matching +#include "software/SfM/ImageCollectionMatcher_AllInMemory.hpp" +#include "software/SfM/ImageCollectionGeometricFilter.hpp" +#include "software/globalSfM/ImageCollection_E_ACRobust.hpp" +#include "software/SfM/pairwiseAdjacencyDisplay.hpp" +#include "software/SfM/SfMIOHelper.hpp" +#include "openMVG/matching/matcher_brute_force.hpp" +#include "openMVG/matching/matcher_kdtree_flann.hpp" +#include "openMVG/matching/indMatch_utils.hpp" + +/// Feature detector and descriptor interface +#include "patented/sift/SIFT.hpp" + +#include "third_party/cmdLine/cmdLine.h" + +#include "third_party/stlplus3/filesystemSimplified/file_system.hpp" +#include "third_party/progress/progress.hpp" + +#include "software/globalSfM/indexedImageGraph.hpp" +#include "software/globalSfM/indexedImageGraphExport.hpp" + +#include +#include +#include +#include +#include + +using namespace openMVG; +using namespace openMVG::matching; +using namespace openMVG::robust; +using namespace std; + +bool testIntrinsicsEquality( + SfMIO::IntrinsicCameraInfo const &ci1, + SfMIO::IntrinsicCameraInfo const &ci2) +{ + return ci1.m_K == ci2.m_K; +} + +int main(int argc, char **argv) +{ + CmdLine cmd; + + std::string sImaDirectory; + std::string sOutDir = ""; + float fDistRatio = .6f; + bool bOctMinus1 = false; + float dPeakThreshold = 0.01f; + + cmd.add( make_option('i', sImaDirectory, "imadir") ); + cmd.add( make_option('o', sOutDir, "outdir") ); + cmd.add( make_option('r', fDistRatio, "distratio") ); + cmd.add( make_option('s', bOctMinus1, "octminus1") ); + cmd.add( make_option('p', dPeakThreshold, "peakThreshold") ); + + try { + if (argc == 1) throw std::string("Invalid command line parameter."); + cmd.process(argc, argv); + } catch(const std::string& s) { + std::cerr << "Usage: " << argv[0] << ' ' + << "[-i|--imadir path] " + << "[-o|--outdir path] " + << "[-r|--distratio 0.6] " + << "[-s|--octminus1 0 or 1] " + << "[-p|--peakThreshold 0.04 -> 0.01] " + << std::endl; + + std::cerr << s << std::endl; + return EXIT_FAILURE; + } + + std::cout << " You called : " < vec_camImageName; + std::vector vec_focalGroup; + if (!openMVG::SfMIO::loadImageList( vec_camImageName, + vec_focalGroup, + sListsFile) ) + { + std::cerr << "\nEmpty image list." << std::endl; + return false; + } + + std::vector::iterator iterF = + std::unique(vec_focalGroup.begin(), vec_focalGroup.end(), testIntrinsicsEquality); + vec_focalGroup.resize( std::distance(vec_focalGroup.begin(), iterF) ); + if (vec_focalGroup.size() == 1) + { + for (size_t i = 0; i < vec_camImageName.size(); ++i) + vec_camImageName[i].m_intrinsicId = 0; + } + else + { + std::cout << "There is more than one focal group in the lists.txt file." << std::endl + << "Only one focal group is supported for the global calibration chain" << std::endl; + return EXIT_FAILURE; + } + +// Using UNIQUE with a special functor and distance == (1) ? + + std::vector vec_fileNames; + std::vector > vec_imagesSize; + for ( std::vector::const_iterator iter_camInfo = vec_camImageName.begin(); + iter_camInfo != vec_camImageName.end(); + iter_camInfo++ ) + { + vec_imagesSize.push_back( std::make_pair( vec_focalGroup[iter_camInfo->m_intrinsicId].m_w, + vec_focalGroup[iter_camInfo->m_intrinsicId].m_h ) ); + vec_fileNames.push_back( stlplus::create_filespec( sImaDirectory, iter_camInfo->m_sImageName) ); + } + + //--------------------------------------- + // b. Compute features and descriptors + // - extract sift features and descriptors + // - if keypoints already computed, re-load them + // - else save features and descriptors on disk + //--------------------------------------- + + typedef Descriptor DescriptorT; + typedef SIOPointFeature FeatureT; + typedef std::vector FeatsT; + typedef vector DescsT; + typedef KeypointSet KeypointSetT; + + { + std::cout << "\n\nEXTRACT FEATURES" << std::endl; + vec_imagesSize.resize(vec_fileNames.size()); + + Image imageRGB; + Image imageGray; + + C_Progress_display my_progress_bar( vec_fileNames.size() ); + for(size_t i=0; i < vec_fileNames.size(); ++i) { + KeypointSetT kpSet; + + std::string sFeat = stlplus::create_filespec(sOutDir, + stlplus::basename_part(vec_fileNames[i]), "feat"); + std::string sDesc = stlplus::create_filespec(sOutDir, + stlplus::basename_part(vec_fileNames[i]), "desc"); + + //Test if descriptors and features was already computed + if (stlplus::file_exists(sFeat) && stlplus::file_exists(sDesc)) { + + if (ReadImage(vec_fileNames[i].c_str(), &imageRGB)) { + vec_imagesSize[i] = make_pair(imageRGB.Width(), imageRGB.Height()); + } + else { + ReadImage(vec_fileNames[i].c_str(), &imageGray); + vec_imagesSize[i] = make_pair(imageGray.Width(), imageGray.Height()); + } + } + else { //Not already computed, so compute and save + + if (ReadImage(vec_fileNames[i].c_str(), &imageRGB)) { + Rgb2Gray(imageRGB, &imageGray); + } + else{ + ReadImage(vec_fileNames[i].c_str(), &imageGray); + } + + // Compute features and descriptors and export them to file + SIFTDetector(imageGray, + kpSet.features(), kpSet.descriptors(), + bOctMinus1, true, dPeakThreshold); + + kpSet.saveToBinFile(sFeat, sDesc); + vec_imagesSize[i] = make_pair(imageGray.Width(), imageRGB.Height()); + } + ++my_progress_bar; + } + } + + //--------------------------------------- + // c. Compute putatives descriptor matches + // - L2 descriptor matching + // - Keep correspondences only if NearestNeighbor ratio is ok + //--------------------------------------- + IndexedMatchPerPair map_PutativesMatches; + // Define the matcher and the used metric (Squared L2) + // ANN matcher could be defined as follow: + typedef flann::L2 MetricT; + typedef ArrayMatcher_Kdtree_Flann MatcherT; + // Brute force matcher is defined as following: + //typedef L2_Vectorized MetricT; + //typedef ArrayMatcherBruteForce MatcherT; + + // If the matches already exists, reload them + if (stlplus::file_exists(sOutDir + "/matches.putative.txt")) + { + PairedIndMatchImport(sOutDir + "/matches.putative.txt", map_PutativesMatches); + std::cout << std::endl << "PUTATIVE MATCHES -- PREVIOUS RESULTS LOADED" << std::endl; + } + else // Compute the putatives matches + { + ImageCollectionMatcher_AllInMemory collectionMatcher(fDistRatio); + if (collectionMatcher.loadData(vec_fileNames, sOutDir)) + { + std::cout << std::endl << "PUTATIVE MATCHES" << std::endl; + collectionMatcher.Match(vec_fileNames, map_PutativesMatches); + //--------------------------------------- + //-- Export putative matches + //--------------------------------------- + std::ofstream file (std::string(sOutDir + "/matches.putative.txt").c_str()); + if (file.is_open()) + PairedIndMatchToStream(map_PutativesMatches, file); + file.close(); + } + } + //-- export putative matches Adjacency matrix + PairWiseMatchingToAdjacencyMatrixSVG(vec_fileNames.size(), + map_PutativesMatches, + stlplus::create_filespec(sOutDir, "PutativeAdjacencyMatrix", "svg")); + + + //--------------------------------------- + // d. Geometric filtering of putatives matches + // - AContrario Estimation of the Essential matrix + // - Use a upper bound for the plausible E matrix + // acontrario estimated threshold + //--------------------------------------- + IndexedMatchPerPair map_GeometricMatches_E; + + GeometricFilter_EMatrix_AC geomFilter_E_AC(vec_focalGroup[0].m_K, 4.0); + ImageCollectionGeometricFilter collectionGeomFilter; + if (collectionGeomFilter.loadData(vec_fileNames, sOutDir)) + { + std::cout << std::endl << " - GEOMETRIC FILTERING - " << std::endl; + collectionGeomFilter.Filter( + geomFilter_E_AC, + map_PutativesMatches, + map_GeometricMatches_E, + vec_imagesSize); + + //-- Perform an additional check to remove pairs with poor overlap + std::vector vec_toRemove; + for (IndexedMatchPerPair::const_iterator iterMap = map_GeometricMatches_E.begin(); + iterMap != map_GeometricMatches_E.end(); ++iterMap) + { + size_t putativePhotometricCount = map_PutativesMatches.find(iterMap->first)->second.size(); + size_t putativeGeometricCount = iterMap->second.size(); + float ratio = putativeGeometricCount / (float)putativePhotometricCount; + if (putativeGeometricCount < 50 || ratio < .30) { + // the pair will be removed + vec_toRemove.push_back(iterMap->first); + } + } + //-- remove discarded pairs + for (std::vector::const_iterator iter = vec_toRemove.begin(); + iter != vec_toRemove.end(); + ++iter) + { + map_GeometricMatches_E.erase(*iter); + } + + //--------------------------------------- + //-- Export geometric filtered matches + //--------------------------------------- + std::ofstream file (string(sOutDir + "/matches.e.txt").c_str()); + if (file.is_open()) + PairedIndMatchToStream(map_GeometricMatches_E, file); + file.close(); + + //-- export Adjacency matrix + std::cout << "\n Export Adjacency Matrix of the pairwise's Epipolar matches" + << std::endl; + PairWiseMatchingToAdjacencyMatrixSVG(vec_fileNames.size(), + map_GeometricMatches_E, + stlplus::create_filespec(sOutDir, "EpipolarAdjacencyMatrix", "svg")); + + //-- Export geometric Essential graph + //--------------------------------------- + typedef lemon::ListGraph Graph; + imageGraph::indexedImageGraph putativeGraph(map_GeometricMatches_E, vec_fileNames); + + // Save the graph before cleaning: + imageGraph::exportToGraphvizData( + stlplus::create_filespec(sOutDir, "essentialGraph"), + putativeGraph.g); + } + return EXIT_SUCCESS; +} + + diff --git a/src/software/globalSfM/mutexSet.hpp b/src/software/globalSfM/mutexSet.hpp new file mode 100644 index 0000000000..251b3c67ab --- /dev/null +++ b/src/software/globalSfM/mutexSet.hpp @@ -0,0 +1,48 @@ + +// Copyright (c) 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#pragma once + +#ifdef USE_CXX11 +#include +using namespace std; +typedef std::mutex mutexT; +#else +#include "third_party/tinythread/fast_mutex.h" +#include "third_party/tinythread/tinythread.h" +using namespace tthread; +typedef tthread::fast_mutex mutexT; +#endif + +namespace openMVG { + +/// ThreadSafe Set thanks to a mutex +template +class MutexSet { + +public: + void discard(const T & value) { + lock_guard guard(m_Mutex); + m_Set.insert(value); + } + + bool isDiscarded(const T & value) const { + lock_guard guard(m_Mutex); + return m_Set.find(value) != m_Set.end(); + } + + size_t size() const { + lock_guard guard(m_Mutex); + return m_Set.size(); + } + +private: + std::set m_Set; + mutable mutexT m_Mutex; +}; + +}; // namespace openMVG From f3413810e2b3f02d5fa8bc9b56807dc04a1183f9 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 19 Jan 2014 11:31:28 +0100 Subject: [PATCH 25/68] Add DrawLineTickness to openMVG/image module. Use it to display K-VLD masks. #61 --- src/openMVG/image/image_drawing.hpp | 103 ++++++++++++++++++ src/openMVG/matching/kvld/kvld_draw.h | 48 ++++++++ .../kvld_filter/kvld_filter.cpp | 26 ++++- 3 files changed, 175 insertions(+), 2 deletions(-) create mode 100644 src/openMVG/matching/kvld/kvld_draw.h diff --git a/src/openMVG/image/image_drawing.hpp b/src/openMVG/image/image_drawing.hpp index 89eabd1533..f92640d73f 100644 --- a/src/openMVG/image/image_drawing.hpp +++ b/src/openMVG/image/image_drawing.hpp @@ -263,6 +263,109 @@ void DrawLine(int xa, int ya, int xb, int yb, const Color& col, Image *pim) SafePutPixel( y, x, col, pim); } +// Draw a serie of circle along the line, the algorithm is slow but accurate +template +void DrawLineTickness(int xa, int ya, int xb, int yb, const Color& col, int thickness, Image *pim) +{ + Image &im = *pim; + int halfThickness = (thickness+1)/2; + + // If one point is outside the image + // Replace the outside point by the intersection of the line and + // the limit (either x=width or y=height). + if (!im.Contains(ya, xa) || !im.Contains(yb, xb)) { + + int width = pim->Width(), height = pim->Height(); + const bool xdir = xa=width) return; + if (xleft<0) { + yleft -= xleft*(yright - yleft)/(xright - xleft); + xleft = 0; + } + if (xright>=width) { + yright -= (xright - width)*(yright - yleft)/(xright - xleft); + xright = float(width) - 1; + } + if (ydown<0 || yup>=height) return; + if (yup<0) { + xup -= yup*(xdown - xup)/(ydown - yup); + yup = 0; + } + if (ydown>=height) { + xdown -= (ydown - height)*(xdown - xup)/(ydown - yup); + ydown = float(height) - 1; + } + + xa = (int) xleft; xb = (int) xright; + ya = (int) yleft; yb = (int) yright; + } + + int xbas, xhaut, ybas, yhaut; + // Check the condition ybas < yhaut. + if (ya <= yb) { + xbas = xa; ybas = ya; + xhaut = xb; yhaut = yb; + } else { + xbas = xb; ybas = yb; + xhaut = xa; yhaut = ya; + } + // Initialize slope. + int x, y, dx, dy, incrmX, incrmY, dp, N, S; + dx = xhaut - xbas; + dy = yhaut - ybas; + if (dx > 0) { // If xhaut > xbas we will increment X. + incrmX = 1; + } else { + incrmX = -1; // else we will decrement X. + dx *= -1; + } + if (dy > 0) { // Positive slope will increment X. + incrmY = 1; + } else { // Negative slope. + incrmY = -1; + } + if (dx >= dy) { + dp = 2 * dy - dx; + S = 2 * dy; + N = 2 * (dy - dx); + y = ybas; + x = xbas; + while (x != xhaut) { + DrawCircle(x, y, halfThickness, col, pim); + x += incrmX; + if (dp <= 0) { // Go in direction of the South Pixel. + dp += S; + } else { // Go to the North. + dp += N; + y+=incrmY; + } + } + } else { + dp = 2 * dx - dy; + S = 2 * dx; + N = 2 * (dx - dy); + x = xbas; + y = ybas; + while (y < yhaut) { + DrawCircle(x, y, halfThickness, col, pim); + y += incrmY; + if (dp <= 0) { // Go in direction of the South Pixel. + dp += S; + } else { // Go to the North. + dp += N; + x += incrmX; + } + } + } + DrawCircle(x, y, halfThickness, col, pim); +} + } //namespace openMVG #endif // OPENMVG_IMAGE_IMAGE_DRAWING_HPP diff --git a/src/openMVG/matching/kvld/kvld_draw.h b/src/openMVG/matching/kvld/kvld_draw.h new file mode 100644 index 0000000000..afef394f53 --- /dev/null +++ b/src/openMVG/matching/kvld/kvld_draw.h @@ -0,0 +1,48 @@ +// Copyright (c) 2012 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#pragma once + +#include "openMVG/image/image.hpp" +#include "openMVG/features/feature.hpp" + +namespace openMVG { + +//-- A slow but accurate way to draw K-VLD lines +void getKVLDMask( + Image< unsigned char > *maskL, + Image< unsigned char > *maskR, + const vector< openMVG::SIOPointFeature > &vec_F1, + const vector< openMVG::SIOPointFeature > &vec_F2, + const vector< Pair >& vec_matches, + const vector< bool >& vec_valide, + const openMVG::Mat& mat_E) +{ + for( int it1 = 0; it1 < vec_matches.size() - 1; it1++ ) + { + for( int it2 = it1 + 1; it2 < vec_matches.size(); it2++ ) + { + if( vec_valide[ it1 ] && vec_valide[ it2 ] && mat_E( it1, it2 ) >= 0 ) + { + const openMVG::SIOPointFeature & l1 = vec_F1[ vec_matches[ it1 ].first ]; + const openMVG::SIOPointFeature & l2 = vec_F1[ vec_matches[ it2 ].first ]; + float l = ( l1.coords() - l2.coords() ).norm(); + int widthL = max( 1.f, l / ( dimension + 1.f ) ); + + DrawLineTickness(l1.x(), l1.y(), l2.x(), l2.y(), 255, widthL, maskL); + + const openMVG::SIOPointFeature & r1 = vec_F2[ vec_matches[ it1 ].second ]; + const openMVG::SIOPointFeature & r2 = vec_F2[ vec_matches[ it2 ].second ]; + float r = ( r1.coords() - r2.coords() ).norm(); + int widthR = max( 1.f, r / ( dimension + 1.f ) ); + + DrawLineTickness(r1.x(), r1.y(), r2.x(), r2.y(), 255, widthR, maskR); + } + } + } +} + +}; // namespace openMVG diff --git a/src/openMVG_Samples/kvld_filter/kvld_filter.cpp b/src/openMVG_Samples/kvld_filter/kvld_filter.cpp index 75d0834bb6..6c52a45bbf 100644 --- a/src/openMVG_Samples/kvld_filter/kvld_filter.cpp +++ b/src/openMVG_Samples/kvld_filter/kvld_filter.cpp @@ -36,6 +36,8 @@ using namespace svg; using namespace std; #include "openMVG/matching/kvld/kvld.h" +#include "openMVG/matching/kvld/kvld_draw.h" + #include "third_party/cmdLine/cmdLine.h" int main(int argc, char **argv) { @@ -173,8 +175,6 @@ int main(int argc, char **argv) { //K-VLD filter - std::vector vec_KVLDMatches; - Image imgA (imageL.GetMat().cast()); Image imgB (imageR.GetMat().cast()); @@ -272,5 +272,27 @@ int main(int argc, char **argv) { svgFile << svgStream.closeSvgFile().str(); svgFile.close(); } + + Image imageOutL = imageL; + Image imageOutR = imageR; + + getKVLDMask( + &imageOutL, &imageOutR, + featsL, featsR, + matchesPair, + valide, + E); + + { + string out_filename = "07_Left-K-VLD-MASK.jpg"; + out_filename = stlplus::create_filespec(sOutDir, out_filename); + WriteImage(out_filename.c_str(), imageOutL); + } + { + string out_filename = "08_Right-K-VLD-MASK.jpg"; + out_filename = stlplus::create_filespec(sOutDir, out_filename); + WriteImage(out_filename.c_str(), imageOutR); + } + return EXIT_SUCCESS; } From 2a6a424655508eb15e6b0be31e31d302066c3a42 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 20 Jan 2014 09:55:33 +0100 Subject: [PATCH 26/68] Fix typo error DrawLineTickness -> DrawLineThickness. #61 --- src/openMVG/image/image_drawing.hpp | 2 +- src/openMVG/matching/kvld/kvld_draw.h | 4 ++-- src/openMVG_Samples/kvld_filter/kvld_filter.cpp | 10 +++++----- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/openMVG/image/image_drawing.hpp b/src/openMVG/image/image_drawing.hpp index f92640d73f..21ff42f4ad 100644 --- a/src/openMVG/image/image_drawing.hpp +++ b/src/openMVG/image/image_drawing.hpp @@ -265,7 +265,7 @@ void DrawLine(int xa, int ya, int xb, int yb, const Color& col, Image *pim) // Draw a serie of circle along the line, the algorithm is slow but accurate template -void DrawLineTickness(int xa, int ya, int xb, int yb, const Color& col, int thickness, Image *pim) +void DrawLineThickness(int xa, int ya, int xb, int yb, const Color& col, int thickness, Image *pim) { Image &im = *pim; int halfThickness = (thickness+1)/2; diff --git a/src/openMVG/matching/kvld/kvld_draw.h b/src/openMVG/matching/kvld/kvld_draw.h index afef394f53..34846fca89 100644 --- a/src/openMVG/matching/kvld/kvld_draw.h +++ b/src/openMVG/matching/kvld/kvld_draw.h @@ -32,14 +32,14 @@ void getKVLDMask( float l = ( l1.coords() - l2.coords() ).norm(); int widthL = max( 1.f, l / ( dimension + 1.f ) ); - DrawLineTickness(l1.x(), l1.y(), l2.x(), l2.y(), 255, widthL, maskL); + DrawLineThickness(l1.x(), l1.y(), l2.x(), l2.y(), 255, widthL, maskL); const openMVG::SIOPointFeature & r1 = vec_F2[ vec_matches[ it1 ].second ]; const openMVG::SIOPointFeature & r2 = vec_F2[ vec_matches[ it2 ].second ]; float r = ( r1.coords() - r2.coords() ).norm(); int widthR = max( 1.f, r / ( dimension + 1.f ) ); - DrawLineTickness(r1.x(), r1.y(), r2.x(), r2.y(), 255, widthR, maskR); + DrawLineThickness(r1.x(), r1.y(), r2.x(), r2.y(), 255, widthR, maskR); } } } diff --git a/src/openMVG_Samples/kvld_filter/kvld_filter.cpp b/src/openMVG_Samples/kvld_filter/kvld_filter.cpp index 6c52a45bbf..a515d0d4ab 100644 --- a/src/openMVG_Samples/kvld_filter/kvld_filter.cpp +++ b/src/openMVG_Samples/kvld_filter/kvld_filter.cpp @@ -277,11 +277,11 @@ int main(int argc, char **argv) { Image imageOutR = imageR; getKVLDMask( - &imageOutL, &imageOutR, - featsL, featsR, - matchesPair, - valide, - E); + &imageOutL, &imageOutR, + featsL, featsR, + matchesPair, + valide, + E); { string out_filename = "07_Left-K-VLD-MASK.jpg"; From ca28d0235cd23d3678fe440b851831050e327a6d Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 20 Jan 2014 09:57:25 +0100 Subject: [PATCH 27/68] Make fast_mutex use windows api on windows (avoid asm). #59 --- src/third_party/tinythread/fast_mutex.h | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/third_party/tinythread/fast_mutex.h b/src/third_party/tinythread/fast_mutex.h index 4d4b7cc431..345710ab3f 100644 --- a/src/third_party/tinythread/fast_mutex.h +++ b/src/third_party/tinythread/fast_mutex.h @@ -38,14 +38,20 @@ freely, subject to the following restrictions: // Check if we can support the assembly language level implementation (otherwise // revert to the system API) -#if (defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))) || \ - (defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64))) || \ - (defined(__GNUC__) && (defined(__ppc__))) - #define _FAST_MUTEX_ASM_ -#else +#if defined(_TTHREAD_WIN32_) +// Only use system mutex (do not use ASM version) #define _FAST_MUTEX_SYS_ +#else + #if (defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))) || \ + (defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64))) || \ + (defined(__GNUC__) && (defined(__ppc__))) && !defined(_TTHREAD_PLATFORM_DEFINED_) + #define _FAST_MUTEX_ASM_ + #else + #define _FAST_MUTEX_SYS_ + #endif #endif + #if defined(_TTHREAD_WIN32_) #ifndef WIN32_LEAN_AND_MEAN #define WIN32_LEAN_AND_MEAN @@ -140,7 +146,7 @@ class fast_mutex { #endif #endif } - + /// Try to lock the mutex. /// The method will try to lock the mutex. If it fails, the function will /// return immediately (non-blocking). From 5464f79679c1e83a3cd11ca8f1317293066a3d41 Mon Sep 17 00:00:00 2001 From: Michael Holroyd Date: Thu, 23 Jan 2014 09:35:07 -0500 Subject: [PATCH 28/68] replace deprecated finite() with isfinite() --- src/openMVG/numeric/numeric.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/openMVG/numeric/numeric.h b/src/openMVG/numeric/numeric.h index dc0c017a0b..a0f86db470 100644 --- a/src/openMVG/numeric/numeric.h +++ b/src/openMVG/numeric/numeric.h @@ -311,7 +311,7 @@ namespace openMVG { #ifdef _WIN32 return _finite(val); #else - return finite(val); + return isfinite(val); #endif } From aabec31aa6dbb91c10618f6c5c5ecc374936f95d Mon Sep 17 00:00:00 2001 From: pmoulon Date: Thu, 23 Jan 2014 17:13:36 +0100 Subject: [PATCH 29/68] Fix compilation regarding continous integration Drone.io build 82. --- src/openMVG/numeric/numeric.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/openMVG/numeric/numeric.h b/src/openMVG/numeric/numeric.h index a0f86db470..46fbe90194 100644 --- a/src/openMVG/numeric/numeric.h +++ b/src/openMVG/numeric/numeric.h @@ -311,7 +311,7 @@ namespace openMVG { #ifdef _WIN32 return _finite(val); #else - return isfinite(val); + return std::isfinite(val); #endif } From 18195280f9930de139320e846eafb0008c6a1ac4 Mon Sep 17 00:00:00 2001 From: Michael Holroyd Date: Thu, 23 Jan 2014 11:40:26 -0500 Subject: [PATCH 30/68] get filename from lists.txt correctly --- src/software/SfM/main_exportKeypoints.cpp | 4 ++++ src/software/SfM/main_exportMatches.cpp | 4 ++++ src/software/SfM/main_exportTracks.cpp | 4 ++++ 3 files changed, 12 insertions(+) diff --git a/src/software/SfM/main_exportKeypoints.cpp b/src/software/SfM/main_exportKeypoints.cpp index cbaebc5209..a387411541 100644 --- a/src/software/SfM/main_exportKeypoints.cpp +++ b/src/software/SfM/main_exportKeypoints.cpp @@ -67,7 +67,11 @@ int main(int argc, char ** argv) std::ifstream in(stlplus::create_filespec(sMatchesDir, "lists", "txt").c_str()); std::string sValue; while(in>>sValue) + { + int n = sValue.find_first_of(';'); + sValue = sValue.substr(0,n); vec_fileNames.push_back(sValue); + } in.close(); } if (vec_fileNames.empty()) { diff --git a/src/software/SfM/main_exportMatches.cpp b/src/software/SfM/main_exportMatches.cpp index bf19b1e3ac..eefd2f47b2 100644 --- a/src/software/SfM/main_exportMatches.cpp +++ b/src/software/SfM/main_exportMatches.cpp @@ -70,7 +70,11 @@ int main(int argc, char ** argv) std::ifstream in(stlplus::create_filespec(sMatchesDir, "lists", "txt").c_str()); std::string sValue; while(in>>sValue) + { + int n = sValue.find_first_of(';'); + sValue = sValue.substr(0,n); vec_fileNames.push_back(sValue); + } in.close(); } if (vec_fileNames.empty()) { diff --git a/src/software/SfM/main_exportTracks.cpp b/src/software/SfM/main_exportTracks.cpp index 956444ee05..1574bbeb67 100644 --- a/src/software/SfM/main_exportTracks.cpp +++ b/src/software/SfM/main_exportTracks.cpp @@ -72,7 +72,11 @@ int main(int argc, char ** argv) std::ifstream in(stlplus::create_filespec(sMatchesDir, "lists", "txt").c_str()); std::string sValue; while(in>>sValue) + { + int n = sValue.find_first_of(';'); + sValue = sValue.substr(0,n); vec_fileNames.push_back(sValue); + } in.close(); } if (vec_fileNames.empty()) { From da517ff5179eeeedeec7632f84a000e67db54b25 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Thu, 23 Jan 2014 18:23:10 +0100 Subject: [PATCH 31/68] Make the 5ptRelativeMotion test more concise in what is evaluated. #67 --- .../solver_essential_five_point_test.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/openMVG/multiview/solver_essential_five_point_test.cpp b/src/openMVG/multiview/solver_essential_five_point_test.cpp index ebfbd59e4b..5256a4d74e 100644 --- a/src/openMVG/multiview/solver_essential_five_point_test.cpp +++ b/src/openMVG/multiview/solver_essential_five_point_test.cpp @@ -213,9 +213,8 @@ TEST(FivePointsRelativePose, Random) { Rs.resize(Es.size()); ts.resize(Es.size()); for (size_t s = 0; s < Es.size(); ++s) { - Vec2 x1Col, x2Col; - x1Col << d.x1.col(0)(0), d.x1.col(0)(1); - x2Col << d.x2.col(0)(0), d.x2.col(0)(1); + Vec2 x1Col = d.x1.col(0); + Vec2 x2Col = d.x2.col(0); CHECK( MotionFromEssentialAndCorrespondence(Es[s], Mat3::Identity(), @@ -248,24 +247,24 @@ TEST(FivePointsRelativePose, test_data_sets) { NViewDataSet d = NRealisticCamerasRing(iNviews, 5, nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K - for(int i=0; i Es, Rs; // Essential, Rotation matrix. vector ts; // Translation matrix. - FivePointsRelativePose(d._x[0], d._x[1], &Es); + FivePointsRelativePose(d._x[0], d._x[i], &Es); // Recover rotation and translation from E. Rs.resize(Es.size()); ts.resize(Es.size()); for (size_t s = 0; s < Es.size(); ++s) { - Vec2 x1Col, x2Col; - x1Col << d._x[0].col(i)(0), d._x[0].col((i+1)%iNviews)(1); - x2Col << d._x[1].col(i)(0), d._x[1].col((i+1)%iNviews)(1); + Vec2 x1Col = d._x[0].col(0); + Vec2 x2Col = d._x[i].col(0); CHECK( MotionFromEssentialAndCorrespondence(Es[s], d._K[0], x1Col, - d._K[1], + d._K[i], x2Col, &Rs[s], &ts[s])); @@ -273,7 +272,7 @@ TEST(FivePointsRelativePose, test_data_sets) { //-- Compute Ground Truth motion Mat3 R; Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero(); - RelativeCameraMotion(d._R[i], d._t[i], d._R[(i+1)%iNviews], d._t[(i+1)%iNviews], &R, &t); + RelativeCameraMotion(d._R[0], d._t[0], d._R[i], d._t[i], &R, &t); // Assert that found relative motion is correct for almost one model. bool bsolution_found = false; From f5de454a33769cc074d4f22b9f0d0b6a281903be Mon Sep 17 00:00:00 2001 From: Fabien Castan Date: Tue, 28 Jan 2014 11:55:12 +0100 Subject: [PATCH 32/68] update gitignore --- .gitignore | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.gitignore b/.gitignore index ca2a38bac7..cfda9f1ed9 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,8 @@ *.toc *.bbl *.blg +*.cbp* +*.layout* +*~ +*.pyc +*.db From f50ac3f9ed259a29bbb5c20d9824c65863ad79ee Mon Sep 17 00:00:00 2001 From: Fabien Castan Date: Tue, 28 Jan 2014 12:01:27 +0100 Subject: [PATCH 33/68] minor code clean * less "using namespace" * explicitly use "std::" * invert some conditions for readability * more doc --- src/openMVG/image/pixel_types.hpp | 1 + .../robust_estimator_ACRansac.hpp | 13 +- src/openMVG/tracks/tracks.hpp | 11 +- .../ImageCollectionMatcher_AllInMemory.hpp | 4 +- src/software/SfM/SfMIncrementalEngine.cpp | 153 +++++++++--------- src/software/SfM/SfMRobust.hpp | 72 ++++++--- 6 files changed, 143 insertions(+), 111 deletions(-) diff --git a/src/openMVG/image/pixel_types.hpp b/src/openMVG/image/pixel_types.hpp index 5fc21b72c9..b1a9d09c05 100644 --- a/src/openMVG/image/pixel_types.hpp +++ b/src/openMVG/image/pixel_types.hpp @@ -66,6 +66,7 @@ class Rgb : public Eigen::Matrix } }; typedef Rgb RGBColor; +typedef Rgb RGBfColor; /// RGBA templated pixel type template diff --git a/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp b/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp index 8cca18b8d0..bf126b19a4 100644 --- a/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp +++ b/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp @@ -121,7 +121,18 @@ static void UniformSample(int sizeSample, (*sample)[i] = vec_index[ (*sample)[i] ]; } -/// ACRANSAC routine (ErrorThreshold, NFA) +/** + * @brief ACRANSAC routine (ErrorThreshold, NFA) + * + * @param[in] kernel model and metric object + * @param[out] vec_inliers points that fit the estimated model + * @param[in] nIter maximum number of consecutive iterations + * @param[out] model returned model if found + * @param[in] precision upper bound of the precision (squared error) + * @param[in] bVerbose display console log + * + * @return (errorMax, minNFA) + */ template std::pair ACRANSAC(const Kernel &kernel, std::vector & vec_inliers, diff --git a/src/openMVG/tracks/tracks.hpp b/src/openMVG/tracks/tracks.hpp index ba24d63875..015bcc52bc 100644 --- a/src/openMVG/tracks/tracks.hpp +++ b/src/openMVG/tracks/tracks.hpp @@ -224,19 +224,20 @@ struct TracksBuilder const set & setB = iter2->second; vector inter; - set_intersection (setA.begin(), setA.end(), setB.begin(), setB.end(), back_inserter(inter)); + set_intersection(setA.begin(), setA.end(), setB.begin(), setB.end(), back_inserter(inter)); if (inter.size() < minMatchesOccurences) copy(inter.begin(), inter.end(), back_inserter(vec_tracksToRemove)); } } sort(vec_tracksToRemove.begin(), vec_tracksToRemove.end()); - std::vector::iterator it = unique (vec_tracksToRemove.begin(), vec_tracksToRemove.end()); + std::vector::iterator it = std::unique(vec_tracksToRemove.begin(), vec_tracksToRemove.end()); vec_tracksToRemove.resize( std::distance(vec_tracksToRemove.begin(), it) ); if (bVerbose) - cout << endl << endl <0; ++k) { + const size_t index = vec_NNRatioIndexes[k]; vec_FilteredMatches.push_back( - IndMatch(vec_nIndice10[vec_NNRatioIndexes[k]*NNN__], - vec_NNRatioIndexes[k]) ); + IndMatch(vec_nIndice10[index*NNN__], index) ); } // Remove duplicates diff --git a/src/software/SfM/SfMIncrementalEngine.cpp b/src/software/SfM/SfMIncrementalEngine.cpp index 2cbd032445..651ffc5b7f 100644 --- a/src/software/SfM/SfMIncrementalEngine.cpp +++ b/src/software/SfM/SfMIncrementalEngine.cpp @@ -24,7 +24,6 @@ #include #include -using namespace openMVG; namespace openMVG{ @@ -83,93 +82,89 @@ bool IncrementalReconstructionEngine::Process() //------------------- //-- Incremental reconstruction //------------------- - bool bOk = true; std::pair initialPairIndex; - if (InitialPairChoice(initialPairIndex)) - { - // Initial pair Essential Matrix and [R|t] estimation. - if(MakeInitialPair3D(initialPairIndex)) - { - _vec_added_order.push_back(initialPairIndex.first); - _vec_added_order.push_back(initialPairIndex.second); + if(! InitialPairChoice(initialPairIndex)) + return false; + + // Initial pair Essential Matrix and [R|t] estimation. + if(! MakeInitialPair3D(initialPairIndex)) + return false; + + _vec_added_order.push_back(initialPairIndex.first); + _vec_added_order.push_back(initialPairIndex.second); - BundleAdjustment(); // Adjust 3D point and camera parameters. + BundleAdjustment(); // Adjust 3D point and camera parameters. - size_t round = 0; - bool bImageAdded = false; - // Compute robust Resection of remaining image - std::vector vec_possible_resection_indexes; - while (FindImagesWithPossibleResection(vec_possible_resection_indexes)) + size_t round = 0; + bool bImageAdded = false; + // Compute robust Resection of remaining image + std::vector vec_possible_resection_indexes; + while (FindImagesWithPossibleResection(vec_possible_resection_indexes)) + { + if (Resection(vec_possible_resection_indexes)) + { + std::ostringstream os; + os << std::setw(8) << std::setfill('0') << round << "_Resection"; + _reconstructorData.exportToPly( stlplus::create_filespec(_sOutDirectory, os.str(), ".ply")); + bImageAdded = true; + } + ++round; + if (bImageAdded && _bUseBundleAdjustment) + { + // Perform BA until all point are under the given precision + do { - if (Resection(vec_possible_resection_indexes)) - { - std::ostringstream os; - os << std::setw(8) << std::setfill('0') << round << "_Resection"; - _reconstructorData.exportToPly( stlplus::create_filespec(_sOutDirectory, os.str(), ".ply")); - bImageAdded = true; - } - ++round; - if (bImageAdded && _bUseBundleAdjustment) - { - // Perform BA until all point are under the given precision - do - { - BundleAdjustment(); - } - while (badTrackRejector(4.0) != 0); - } + BundleAdjustment(); } + while (badTrackRejector(4.0) != 0); + } + } - //-- Reconstruction done. - //-- Display some statistics - std::cout << "\n\n-------------------------------" << "\n" - << "-- Structure from Motion (statistics):\n" - << "-- #Camera calibrated: " << _reconstructorData.map_Camera.size() - << " from " << _vec_camImageNames.size() << " input images.\n" - << "-- #Tracks, #3D points: " << _reconstructorData.map_3DPoints.size() << "\n" - << "-------------------------------" << "\n"; + //-- Reconstruction done. + //-- Display some statistics + std::cout << "\n\n-------------------------------" << "\n" + << "-- Structure from Motion (statistics):\n" + << "-- #Camera calibrated: " << _reconstructorData.map_Camera.size() + << " from " << _vec_camImageNames.size() << " input images.\n" + << "-- #Tracks, #3D points: " << _reconstructorData.map_3DPoints.size() << "\n" + << "-------------------------------" << "\n"; - Histogram h; - ComputeResidualsHistogram(&h); - std::cout << "\nHistogram of residuals:" << h.ToString() << std::endl; + Histogram h; + ComputeResidualsHistogram(&h); + std::cout << "\nHistogram of residuals:" << h.ToString() << std::endl; - if (_bHtmlReport) - { - using namespace htmlDocument; - std::ostringstream os; - os << "Structure from Motion process finished."; - _htmlDocStream->pushInfo("
"); - _htmlDocStream->pushInfo(htmlMarkup("h1",os.str())); - - os.str(""); - os << "-------------------------------" << "
" - << "-- Structure from Motion (statistics):
" - << "-- #Camera calibrated: " << _reconstructorData.map_Camera.size() - << " from " <<_vec_camImageNames.size() << " input images.
" - << "-- #Tracks, #3D points: " << _reconstructorData.map_3DPoints.size() << "
" - << "-------------------------------" << "
"; - _htmlDocStream->pushInfo(os.str()); - - _htmlDocStream->pushInfo(htmlMarkup("h2","Histogram of reprojection-residuals")); - - std::vector xBin = h.GetXbinsValue(); - std::pair< std::pair, std::pair > range; - range = autoJSXGraphViewport(xBin, h.GetHist()); - - htmlDocument::JSXGraphWrapper jsxGraph; - jsxGraph.init("3DtoImageResiduals",600,300); - jsxGraph.addXYChart(xBin, h.GetHist(), "line,point"); - jsxGraph.UnsuspendUpdate(); - jsxGraph.setViewport(range); - jsxGraph.close(); - _htmlDocStream->pushInfo(jsxGraph.toStr()); - } - } - else { // (MakeInitialPair3D(initialPairIndex)) failed - bOk = false; - } + if (_bHtmlReport) + { + using namespace htmlDocument; + std::ostringstream os; + os << "Structure from Motion process finished."; + _htmlDocStream->pushInfo("
"); + _htmlDocStream->pushInfo(htmlMarkup("h1",os.str())); + + os.str(""); + os << "-------------------------------" << "
" + << "-- Structure from Motion (statistics):
" + << "-- #Camera calibrated: " << _reconstructorData.map_Camera.size() + << " from " <<_vec_camImageNames.size() << " input images.
" + << "-- #Tracks, #3D points: " << _reconstructorData.map_3DPoints.size() << "
" + << "-------------------------------" << "
"; + _htmlDocStream->pushInfo(os.str()); + + _htmlDocStream->pushInfo(htmlMarkup("h2","Histogram of reprojection-residuals")); + + std::vector xBin = h.GetXbinsValue(); + std::pair< std::pair, std::pair > range; + range = autoJSXGraphViewport(xBin, h.GetHist()); + + htmlDocument::JSXGraphWrapper jsxGraph; + jsxGraph.init("3DtoImageResiduals",600,300); + jsxGraph.addXYChart(xBin, h.GetHist(), "line,point"); + jsxGraph.UnsuspendUpdate(); + jsxGraph.setViewport(range); + jsxGraph.close(); + _htmlDocStream->pushInfo(jsxGraph.toStr()); } - return bOk; + return true; } bool IncrementalReconstructionEngine::ReadInputData() diff --git a/src/software/SfM/SfMRobust.hpp b/src/software/SfM/SfMRobust.hpp index f44504887e..2ad3e9ad4a 100644 --- a/src/software/SfM/SfMRobust.hpp +++ b/src/software/SfM/SfMRobust.hpp @@ -19,21 +19,34 @@ #include "openMVG/multiview/triangulation.hpp" #include "openMVG/cameras/PinholeCamera.hpp" -using namespace openMVG; - #include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" #include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp" +namespace openMVG{ +namespace SfMRobust{ + using namespace openMVG::matching; using namespace openMVG::robust; static const size_t ACRANSAC_ITER = 4096; -namespace openMVG{ -namespace SfMRobust{ -/// Estimate the essential matrix from point matches and K matrices. -bool robustEssential(const Mat3 & K1, const Mat3 & K2, +/** + * @brief Estimate the essential matrix from point matches and K matrices. + * + * @param[in] K1 camera 1 intrinsics + * @param[in] K2 camera 2 intrinsics + * @param[in] x1 camera 1 image points + * @param[in] x2 camera 2 image points + * @param[out] pE essential matrix (can be NULL) + * @param[out] pvec_inliers inliers indices (can be empty) + * @param[in] size_ima1 width, height of image 1 + * @param[in] size_ima2 width, height of image 2 + * @param[out] errorMax upper bound of the reprojection error of the found solution + * @param[in] precision upper bound of the desired solution + */ +bool robustEssential( + const Mat3 & K1, const Mat3 & K2, const Mat & x1, const Mat & x2, Mat3 * pE, std::vector * pvec_inliers, const std::pair & size_ima1, @@ -58,22 +71,31 @@ bool robustEssential(const Mat3 & K1, const Mat3 & K2, x2, size_ima2.first, size_ima2.second, K1, K2); // Robustly estimation of the Essential matrix and it's precision - std::pair ACRansacOut = ACRANSAC(kernel, *pvec_inliers, + std::pair acRansacOut = ACRANSAC(kernel, *pvec_inliers, ACRANSAC_ITER, pE, precision, false); - *errorMax = ACRansacOut.first; + *errorMax = acRansacOut.first; return pvec_inliers->size() > 2.5 * SolverType::MINIMUM_SAMPLES; } -/// Estimate the best possible Rotation/Translation from E -/// Four are possible, keep the one with most of the point in front. +/** + * @brief Estimate the best possible Rotation/Translation from E. + * Four are possible, keep the one with most of the point in front. + * + * @param[in] K1 camera 1 intrinsics + * @param[in] K2 camera 2 intrinsics + * @param[in] x1 camera 1 image points + * @param[in] x2 camera 2 image points + * @param[in] E essential matrix + * @param[in] vec_inliers inliers indices + * @param[out] R estimated rotation + * @param[out] t estimated translation + */ bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2, const Mat & x1, const Mat & x2, const Mat3 & E, const std::vector & vec_inliers, Mat3 * R, Vec3 * t) { - bool bOk = false; - // Accumulator to find the best solution std::vector f(4, 0); @@ -94,37 +116,39 @@ bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2, Vec3 t1 = Vec3::Zero(); P_From_KRt(K1, R1, t1, &P1); - for (int i = 0; i < 4; ++i) { + for (int i = 0; i < 4; ++i) + { const Mat3 &R2 = Rs[i]; const Vec3 &t2 = ts[i]; P_From_KRt(K2, R2, t2, &P2); Vec3 X; - for (size_t k = 0; k < vec_inliers.size(); ++k) { + for (size_t k = 0; k < vec_inliers.size(); ++k) + { const Vec2 & x1_ = x1.col(vec_inliers[k]), & x2_ = x2.col(vec_inliers[k]); TriangulateDLT(P1, x1_, P2, x2_, &X); // Test if point is front to the two cameras. - if (Depth(R1, t1, X) > 0 && Depth(R2, t2, X) > 0) { + if (Depth(R1, t1, X) > 0 && Depth(R2, t2, X) > 0) + { ++f[i]; } } } // Check the solution: std::vector::iterator iter = max_element(f.begin(), f.end()); - if(*iter != 0) { - size_t index = std::distance(f.begin(),iter); - (*R) = Rs[index]; - (*t) = ts[index]; - bOk = true; - } - else { + if(*iter == 0) + { std::cerr << std::endl << "/!\\There is no right solution," <<" probably intermediate results are not correct or no points" <<" in front of both cameras" << std::endl; - bOk = false; + return false; } - return bOk; + size_t index = std::distance(f.begin(),iter); + (*R) = Rs[index]; + (*t) = ts[index]; + + return true; } /// Triangulate a set of points between two view From 9cefd0f0bb30e720a94d9a9a0c157c79fc545002 Mon Sep 17 00:00:00 2001 From: Fabien Castan Date: Tue, 28 Jan 2014 15:18:51 +0100 Subject: [PATCH 34/68] no using namespace in header --- src/software/SfM/ImageCollection_F_ACRobust.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/software/SfM/ImageCollection_F_ACRobust.hpp b/src/software/SfM/ImageCollection_F_ACRobust.hpp index 5f07e58903..c5731ec69e 100644 --- a/src/software/SfM/ImageCollection_F_ACRobust.hpp +++ b/src/software/SfM/ImageCollection_F_ACRobust.hpp @@ -11,11 +11,9 @@ #include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" #include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp" -using namespace openMVG; -using namespace openMVG::robust; - #include + //-- A contrario Functor to filter putative corresponding points struct GeometricFilter_FMatrix_AC { @@ -32,6 +30,8 @@ struct GeometricFilter_FMatrix_AC const std::pair & imgSizeB, std::vector & vec_inliers) const { + using namespace openMVG; + using namespace openMVG::robust; vec_inliers.resize(0); // Define the AContrario adapted Fundamental matrix solver typedef ACKernelAdaptor< @@ -60,3 +60,4 @@ struct GeometricFilter_FMatrix_AC double m_dPrecision; //upper_bound of the precision size_t m_stIteration; //maximal number of iteration used }; + From 5a2b67cd3cc7457d3244ec34f0d20bfc73a59681 Mon Sep 17 00:00:00 2001 From: Fabien Castan Date: Tue, 28 Jan 2014 15:19:15 +0100 Subject: [PATCH 35/68] compilation fix --- src/software/globalSfM/SfMGlobal_tij_computation.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/globalSfM/SfMGlobal_tij_computation.hpp b/src/software/globalSfM/SfMGlobal_tij_computation.hpp index 9d2822903c..8d060dd0bb 100644 --- a/src/software/globalSfM/SfMGlobal_tij_computation.hpp +++ b/src/software/globalSfM/SfMGlobal_tij_computation.hpp @@ -72,7 +72,7 @@ bool estimate_T_triplet( TrifocalTensorModel T; Mat3 Kinv = K.inverse(); dPrecision = dPrecision * Kinv(0,0) * Kinv(0,0);//std::numeric_limits::infinity(); - std::pair acStat = ACRANSAC(kernel, vec_inliers, ORSA_ITER, &T, dPrecision, false); + std::pair acStat = robust::ACRANSAC(kernel, vec_inliers, ORSA_ITER, &T, dPrecision, false); dPrecision = acStat.first; //-- Export data in order to have an idea of the precision of the estimates From 507f566668bf6f8fdf2b2793a8e6a25060b9bf68 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Thu, 30 Jan 2014 19:39:13 +0100 Subject: [PATCH 36/68] update file to make compilation easier --- src/openMVG/matching/kvld/kvld_draw.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/openMVG/matching/kvld/kvld_draw.h b/src/openMVG/matching/kvld/kvld_draw.h index afef394f53..d31891954d 100644 --- a/src/openMVG/matching/kvld/kvld_draw.h +++ b/src/openMVG/matching/kvld/kvld_draw.h @@ -8,6 +8,7 @@ #include "openMVG/image/image.hpp" #include "openMVG/features/feature.hpp" +#include namespace openMVG { @@ -15,10 +16,10 @@ namespace openMVG { void getKVLDMask( Image< unsigned char > *maskL, Image< unsigned char > *maskR, - const vector< openMVG::SIOPointFeature > &vec_F1, - const vector< openMVG::SIOPointFeature > &vec_F2, - const vector< Pair >& vec_matches, - const vector< bool >& vec_valide, + const std::vector< openMVG::SIOPointFeature > &vec_F1, + const std::vector< openMVG::SIOPointFeature > &vec_F2, + const std::vector< Pair >& vec_matches, + const std::vector< bool >& vec_valide, const openMVG::Mat& mat_E) { for( int it1 = 0; it1 < vec_matches.size() - 1; it1++ ) @@ -30,14 +31,14 @@ void getKVLDMask( const openMVG::SIOPointFeature & l1 = vec_F1[ vec_matches[ it1 ].first ]; const openMVG::SIOPointFeature & l2 = vec_F1[ vec_matches[ it2 ].first ]; float l = ( l1.coords() - l2.coords() ).norm(); - int widthL = max( 1.f, l / ( dimension + 1.f ) ); + int widthL = std::max( 1.f, l / ( dimension + 1.f ) ); DrawLineTickness(l1.x(), l1.y(), l2.x(), l2.y(), 255, widthL, maskL); const openMVG::SIOPointFeature & r1 = vec_F2[ vec_matches[ it1 ].second ]; const openMVG::SIOPointFeature & r2 = vec_F2[ vec_matches[ it2 ].second ]; float r = ( r1.coords() - r2.coords() ).norm(); - int widthR = max( 1.f, r / ( dimension + 1.f ) ); + int widthR = std::max( 1.f, r / ( dimension + 1.f ) ); DrawLineTickness(r1.x(), r1.y(), r2.x(), r2.y(), 255, widthR, maskR); } From 5dd113932fa99cbc0604f7640254c076015515ed Mon Sep 17 00:00:00 2001 From: pmoulon Date: Thu, 30 Jan 2014 19:40:36 +0100 Subject: [PATCH 37/68] make compilation easier on windows --- .../lInfinityCV/resection_test.cpp | 2 + src/openMVG/matching/matcher_brute_force.hpp | 4 +- src/openMVG/matching/matcher_kdtree_flann.hpp | 5 +- .../robust_estimator_ACRansac.hpp | 2 +- src/openMVG/tracks/tracks.hpp | 8 ++- src/software/SfM/SfMIncrementalEngine.cpp | 58 +++++++++++-------- src/software/SfM/SfMIncrementalEngine.hpp | 12 +--- src/software/SfM/pairwiseAdjacencyDisplay.hpp | 2 +- src/third_party/tinythread/fast_mutex.h | 16 +++-- 9 files changed, 65 insertions(+), 44 deletions(-) diff --git a/src/openMVG/linearProgramming/lInfinityCV/resection_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/resection_test.cpp index 130691c099..77caec80d1 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/resection_test.cpp +++ b/src/openMVG/linearProgramming/lInfinityCV/resection_test.cpp @@ -14,7 +14,9 @@ #include "openMVG/linearProgramming/linearProgrammingInterface.hpp" #include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#ifdef OPENMVG_HAVE_MOSEK #include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" +#endif // OPENMVG_HAVE_MOSEK #include "openMVG/linearProgramming/bisectionLP.hpp" #include "openMVG/linearProgramming/lInfinityCV/resection.hpp" diff --git a/src/openMVG/matching/matcher_brute_force.hpp b/src/openMVG/matching/matcher_brute_force.hpp index 057d51a194..dd0d07577e 100644 --- a/src/openMVG/matching/matcher_brute_force.hpp +++ b/src/openMVG/matching/matcher_brute_force.hpp @@ -25,7 +25,9 @@ class ArrayMatcherBruteForce : public ArrayMatcher typedef typename Metric::ResultType DistanceType; ArrayMatcherBruteForce() {} - virtual ~ArrayMatcherBruteForce() {} + virtual ~ArrayMatcherBruteForce() { + memMapping.reset(); + } /** * Build the matching structure diff --git a/src/openMVG/matching/matcher_kdtree_flann.hpp b/src/openMVG/matching/matcher_kdtree_flann.hpp index 9c06ade588..7970af63df 100644 --- a/src/openMVG/matching/matcher_kdtree_flann.hpp +++ b/src/openMVG/matching/matcher_kdtree_flann.hpp @@ -29,7 +29,10 @@ class ArrayMatcher_Kdtree_Flann : public ArrayMatcher ArrayMatcher_Kdtree_Flann() {} - virtual ~ArrayMatcher_Kdtree_Flann() {} + virtual ~ArrayMatcher_Kdtree_Flann() { + _datasetM.reset(); + _index.reset(); + } /** * Build the matching structure diff --git a/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp b/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp index 8cca18b8d0..65ceaab305 100644 --- a/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp +++ b/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp @@ -95,7 +95,7 @@ static ErrorIndex bestNFA( ErrorIndex bestIndex(std::numeric_limits::infinity(), startIndex); const size_t n = e.size(); for(size_t k=startIndex+1; k<=n && e[k-1].first<=maxThreshold; ++k) { - float logalpha = logalpha0 + multError *log10(e[k-1].first + std::numeric_limits::min()); + double logalpha = logalpha0 + multError *log10(e[k-1].first + std::numeric_limits::min()); ErrorIndex index(loge0 + logalpha * (double)(k-startIndex) + logc_n[k] + diff --git a/src/openMVG/tracks/tracks.hpp b/src/openMVG/tracks/tracks.hpp index ba24d63875..04b63f5831 100644 --- a/src/openMVG/tracks/tracks.hpp +++ b/src/openMVG/tracks/tracks.hpp @@ -38,7 +38,6 @@ using namespace lemon; #include "openMVG/matching/indMatch.hpp" -using namespace openMVG::matching; #include #include @@ -47,6 +46,10 @@ using namespace openMVG::matching; #include #include +namespace openMVG { + +using namespace openMVG::matching; + /// Lightweight copy of the flat_map of BOOST library /// Use a vector to speed up insertion (preallocated array) template @@ -75,13 +78,12 @@ class flat_pair_map static bool superiorToFirst(const P &a, const T1 &b) {return a.first) typedef std::map< std::pair, std::vector > mapPairWiseMatches; +typedef tracks::mapPairWiseMatches STLPairWiseMatches; // Data structure to store a track: collection of {ImageId,FeatureId} // The corresponding image points with their imageId and FeatureId. diff --git a/src/software/SfM/SfMIncrementalEngine.cpp b/src/software/SfM/SfMIncrementalEngine.cpp index 2cbd032445..a62f7dd032 100644 --- a/src/software/SfM/SfMIncrementalEngine.cpp +++ b/src/software/SfM/SfMIncrementalEngine.cpp @@ -13,6 +13,11 @@ #include "openMVG/bundle_adjustment/pinhole_brown_Rt_ceres_functor.hpp" #include "openMVG/bundle_adjustment/problem_data_container.hpp" +//-- Feature tracking +#include "openMVG/tracks/tracks.hpp" +#include "openMVG/matching/indMatch.hpp" +using namespace openMVG::tracks; + #include "third_party/stlplus3/filesystemSimplified/file_system.hpp" #include "third_party/vectorGraphics/svgDrawer.hpp" #include "third_party/stlAddition/stlMap.hpp" @@ -339,7 +344,7 @@ bool IncrementalReconstructionEngine::InitialPairChoice( std::pair vec_NbMatchesPerPair; - for (STLPairWiseMatches::const_iterator iter = _map_Matches_F.begin(); + for (openMVG::tracks::STLPairWiseMatches::const_iterator iter = _map_Matches_F.begin(); iter != _map_Matches_F.end(); ++iter) { vec_NbMatchesPerPair.push_back(iter->second.size()); @@ -351,7 +356,7 @@ bool IncrementalReconstructionEngine::InitialPairChoice( std::pairfirst.first << "," << iter->first.second <<")\t\t" << iter->second.size() << " matches" << std::endl; @@ -371,7 +376,7 @@ bool IncrementalReconstructionEngine::InitialPairChoice( std::pair set_imageIndex; set_imageIndex.insert(I); set_imageIndex.insert(J); @@ -411,8 +416,10 @@ bool IncrementalReconstructionEngine::MakeInitialPair3D(const std::pairsecond.begin(); @@ -498,8 +505,10 @@ bool IncrementalReconstructionEngine::MakeInitialPair3D(const std::pair vec_index; - for (STLMAPTracks::const_iterator iterT = map_tracksCommon.begin(); - iterT != map_tracksCommon.end(); ++iterT) + for (openMVG::tracks::STLMAPTracks::const_iterator + iterT = map_tracksCommon.begin(); + iterT != map_tracksCommon.end(); + ++iterT) { tracks::submapTrack::const_iterator iter = iterT->second.begin(); tracks::submapTrack::const_iterator iter2 = iterT->second.begin(); @@ -516,7 +525,8 @@ bool IncrementalReconstructionEngine::MakeInitialPair3D(const std::pair set_imageIndex; set_imageIndex.insert(imageIndex); TracksUtilsMap::GetTracksInImages(set_imageIndex, _map_tracks, map_tracksCommon); @@ -727,7 +737,7 @@ bool IncrementalReconstructionEngine::Resection(size_t imageIndex) << "-------------------------------" << std::endl; // Compute 2D - 3D possible content - STLMAPTracks map_tracksCommon; + openMVG::tracks::STLMAPTracks map_tracksCommon; std::set set_imageIndex; set_imageIndex.insert(imageIndex); TracksUtilsMap::GetTracksInImages(set_imageIndex, _map_tracks, map_tracksCommon); @@ -790,13 +800,11 @@ bool IncrementalReconstructionEngine::Resection(size_t imageIndex) const openMVG::SfMIO::IntrinsicCameraInfo & intrinsicCam = _vec_intrinsicGroups[_vec_camImageNames[imageIndex].m_intrinsicId]; bool bResection = SfMRobust::robustResection( - std::make_pair( intrinsicCam.m_w, - intrinsicCam.m_h ), + std::make_pair( intrinsicCam.m_w, intrinsicCam.m_h ), pt2D, pt3D, &vec_inliers, - // If intrinsics guess exist use it, else use standard 6 points pose resection + // If intrinsics guess exist use it, else use a standard 6 points pose resection (intrinsicCam.m_bKnownIntrinsic == true) ? & intrinsicCam.m_K : NULL, - //NULL, &P, &errorMax); std::cout << std::endl @@ -1145,9 +1153,9 @@ void IncrementalReconstructionEngine::ColorizeTracks(std::vector & vec_col vec_color.resize(_map_reconstructed.size()); // The track list that will be colored (point removed during the process) - STLMAPTracks mapTrackToColorRef(_map_reconstructed); - STLMAPTracks::iterator iterTBegin = mapTrackToColorRef.begin(); - STLMAPTracks mapTrackToColor(_map_reconstructed); + openMVG::tracks::STLMAPTracks mapTrackToColorRef(_map_reconstructed); + openMVG::tracks::STLMAPTracks::iterator iterTBegin = mapTrackToColorRef.begin(); + openMVG::tracks::STLMAPTracks mapTrackToColor(_map_reconstructed); while( !mapTrackToColor.empty() ) { // Find the most representative image @@ -1155,8 +1163,10 @@ void IncrementalReconstructionEngine::ColorizeTracks(std::vector & vec_col // b. Sort to find the most representative image std::map map_IndexCardinal; // ImageIndex, Cardinal - for (STLMAPTracks::const_iterator iterT = mapTrackToColor.begin(); - iterT != mapTrackToColor.end(); ++iterT) + for (openMVG::tracks::STLMAPTracks::const_iterator + iterT = mapTrackToColor.begin(); + iterT != mapTrackToColor.end(); + ++iterT) { const size_t trackId = iterT->first; const tracks::submapTrack & track = mapTrackToColor[trackId]; @@ -1194,8 +1204,10 @@ void IncrementalReconstructionEngine::ColorizeTracks(std::vector & vec_col // Iterate through the track std::set set_toRemove; - for (STLMAPTracks::const_iterator iterT = mapTrackToColor.begin(); - iterT != mapTrackToColor.end(); ++iterT) + for (openMVG::tracks::STLMAPTracks::const_iterator + iterT = mapTrackToColor.begin(); + iterT != mapTrackToColor.end(); + ++iterT) { const size_t trackId = iterT->first; const tracks::submapTrack & track = mapTrackToColor[trackId]; @@ -1249,7 +1261,7 @@ void IncrementalReconstructionEngine::BundleAdjustment() } std::cout << "nbCams: " << nbCams << std::endl - << "nbIntrinsics:" << nbIntrinsics << std::endl + << "nbIntrinsics: " << nbIntrinsics << std::endl << "nbPoints3D: " << nbPoints3D << std::endl << "measurements: " << nbmeasurements << std::endl; diff --git a/src/software/SfM/SfMIncrementalEngine.hpp b/src/software/SfM/SfMIncrementalEngine.hpp index 19f2a2253b..c465512e3d 100644 --- a/src/software/SfM/SfMIncrementalEngine.hpp +++ b/src/software/SfM/SfMIncrementalEngine.hpp @@ -14,10 +14,7 @@ #include "software/SfM/SfMReconstructionData.hpp" #include "software/SfM/SfMIOHelper.hpp" #include "openMVG/features/features.hpp" - -#include "openMVG/tracks/tracks.hpp" -#include "openMVG/matching/indMatch.hpp" -using namespace openMVG::tracks; +#include #include @@ -133,10 +130,8 @@ class IncrementalReconstructionEngine : public ReconstructionEngine // Intrinsic Id per imageId std::map _map_IntrinsicIdPerImageId; - - typedef tracks::mapPairWiseMatches STLPairWiseMatches; - STLPairWiseMatches _map_Matches_F; // pairwise matches for Fundamental model - + //-- Visibility information + openMVG::tracks::STLPairWiseMatches _map_Matches_F; // pairwise matches for Fundamental model openMVG::tracks::STLMAPTracks _map_tracks; // reconstructed track (visibility per 3D point) //-- configuration of the reconstruction @@ -158,7 +153,6 @@ class IncrementalReconstructionEngine : public ReconstructionEngine /// List of images that belong to a common intrinsic group std::map > _map_ImagesIdPerIntrinsicGroup; std::map _map_IntrinsicsPerGroup; - //std::map _map_Camera; // ----- // Reporting .. diff --git a/src/software/SfM/pairwiseAdjacencyDisplay.hpp b/src/software/SfM/pairwiseAdjacencyDisplay.hpp index 27f4be181c..817e4202f5 100644 --- a/src/software/SfM/pairwiseAdjacencyDisplay.hpp +++ b/src/software/SfM/pairwiseAdjacencyDisplay.hpp @@ -36,7 +36,7 @@ void PairWiseMatchingToAdjacencyMatrixSVG(const size_t NbImages, // Display as a tooltip: (IndexI, IndexJ NbMatches) std::ostringstream os; os << "(" << J << "," << I << " " << iterSearch->second.size() <<")"; - svgStream.drawSquare(J*scaleFactor, I*scaleFactor, scaleFactor/2.0, + svgStream.drawSquare(J*scaleFactor, I*scaleFactor, scaleFactor/2.0f, svgStyle().fill("blue").noStroke()); } // HINT : THINK ABOUT OPACITY [0.4 -> 1.0] TO EXPRESS MATCH COUNT } diff --git a/src/third_party/tinythread/fast_mutex.h b/src/third_party/tinythread/fast_mutex.h index 4d4b7cc431..dcd0293a2f 100644 --- a/src/third_party/tinythread/fast_mutex.h +++ b/src/third_party/tinythread/fast_mutex.h @@ -38,14 +38,20 @@ freely, subject to the following restrictions: // Check if we can support the assembly language level implementation (otherwise // revert to the system API) -#if (defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))) || \ - (defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64))) || \ - (defined(__GNUC__) && (defined(__ppc__))) - #define _FAST_MUTEX_ASM_ -#else +#if defined(_TTHREAD_WIN32_) +// Only use system mutex (do not use ASM version) #define _FAST_MUTEX_SYS_ +#else + #if (defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__))) || \ + (defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64))) || \ + (defined(__GNUC__) && (defined(__ppc__))) && !defined(_TTHREAD_PLATFORM_DEFINED_) + #define _FAST_MUTEX_ASM_ + #else + #define _FAST_MUTEX_SYS_ + #endif #endif + #if defined(_TTHREAD_WIN32_) #ifndef WIN32_LEAN_AND_MEAN #define WIN32_LEAN_AND_MEAN From 1391d8093354892c546c1c2b7d9b83921a5ebef9 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Thu, 30 Jan 2014 20:02:38 +0100 Subject: [PATCH 38/68] Fix loading of image names from lists.txt file. --- .../ImageCollectionGeometricFilter.hpp | 0 .../ImageCollectionMatcher.hpp | 0 .../ImageCollectionMatcher_AllInMemory.hpp | 0 .../ImageCollection_E_ACRobust.hpp | 0 .../ImageCollection_F_ACRobust.hpp | 0 src/software/SfM/SfMIOHelper.hpp | 26 ++++++++++++++- src/software/SfM/main_exportKeypoints.cpp | 33 +++++-------------- src/software/SfM/main_exportMatches.cpp | 13 +++----- src/software/SfM/main_exportTracks.cpp | 13 +++----- 9 files changed, 42 insertions(+), 43 deletions(-) rename src/{software/SfM => openMVG/matching_image_collection}/ImageCollectionGeometricFilter.hpp (100%) rename src/{software/SfM => openMVG/matching_image_collection}/ImageCollectionMatcher.hpp (100%) rename src/{software/SfM => openMVG/matching_image_collection}/ImageCollectionMatcher_AllInMemory.hpp (100%) rename src/{software/globalSfM => openMVG/matching_image_collection}/ImageCollection_E_ACRobust.hpp (100%) rename src/{software/SfM => openMVG/matching_image_collection}/ImageCollection_F_ACRobust.hpp (100%) diff --git a/src/software/SfM/ImageCollectionGeometricFilter.hpp b/src/openMVG/matching_image_collection/ImageCollectionGeometricFilter.hpp similarity index 100% rename from src/software/SfM/ImageCollectionGeometricFilter.hpp rename to src/openMVG/matching_image_collection/ImageCollectionGeometricFilter.hpp diff --git a/src/software/SfM/ImageCollectionMatcher.hpp b/src/openMVG/matching_image_collection/ImageCollectionMatcher.hpp similarity index 100% rename from src/software/SfM/ImageCollectionMatcher.hpp rename to src/openMVG/matching_image_collection/ImageCollectionMatcher.hpp diff --git a/src/software/SfM/ImageCollectionMatcher_AllInMemory.hpp b/src/openMVG/matching_image_collection/ImageCollectionMatcher_AllInMemory.hpp similarity index 100% rename from src/software/SfM/ImageCollectionMatcher_AllInMemory.hpp rename to src/openMVG/matching_image_collection/ImageCollectionMatcher_AllInMemory.hpp diff --git a/src/software/globalSfM/ImageCollection_E_ACRobust.hpp b/src/openMVG/matching_image_collection/ImageCollection_E_ACRobust.hpp similarity index 100% rename from src/software/globalSfM/ImageCollection_E_ACRobust.hpp rename to src/openMVG/matching_image_collection/ImageCollection_E_ACRobust.hpp diff --git a/src/software/SfM/ImageCollection_F_ACRobust.hpp b/src/openMVG/matching_image_collection/ImageCollection_F_ACRobust.hpp similarity index 100% rename from src/software/SfM/ImageCollection_F_ACRobust.hpp rename to src/openMVG/matching_image_collection/ImageCollection_F_ACRobust.hpp diff --git a/src/software/SfM/SfMIOHelper.hpp b/src/software/SfM/SfMIOHelper.hpp index a09b9438c8..4e48c8100a 100644 --- a/src/software/SfM/SfMIOHelper.hpp +++ b/src/software/SfM/SfMIOHelper.hpp @@ -124,7 +124,7 @@ static bool loadImageList( std::vector & vec_camImageName, case 6 : // a camera with exif data not found in the database { oss.clear(); oss.str(vec_str[3]); - double focal; + float focal; oss >> focal; intrinsicCamInfo.m_focal = focal; intrinsicCamInfo.m_bKnownIntrinsic = true; @@ -192,6 +192,30 @@ static bool loadImageList( std::vector & vec_camImageName, return !(vec_camImageName.empty()); } +//-- Load an image list file but only return camera image names +static bool loadImageList( std::vector & vec_camImageName, + std::string sListFileName, + bool bVerbose = true ) +{ + vec_camImageName.clear(); + std::vector vec_camImageIntrinsicInfo; + std::vector vec_focalGroup; + if (loadImageList( vec_camImageIntrinsicInfo, + vec_focalGroup, + sListFileName, + bVerbose) ) + { + for ( std::vector::const_iterator + iter_camInfo = vec_camImageIntrinsicInfo.begin(); + iter_camInfo != vec_camImageIntrinsicInfo.end(); + iter_camInfo++ ) + { + vec_camImageName.push_back( iter_camInfo->m_sImageName ); + } + } + return (!vec_camImageName.empty()); +} + } // namespace SfMIO } // namespace openMVG diff --git a/src/software/SfM/main_exportKeypoints.cpp b/src/software/SfM/main_exportKeypoints.cpp index cbaebc5209..058de9eb62 100644 --- a/src/software/SfM/main_exportKeypoints.cpp +++ b/src/software/SfM/main_exportKeypoints.cpp @@ -10,6 +10,7 @@ #include "openMVG/image/image.hpp" #include "openMVG/features/features.hpp" +#include "software/SfM/SfMIOHelper.hpp" #include "third_party/cmdLine/cmdLine.h" #include "third_party/stlplus3/filesystemSimplified/file_system.hpp" #include "third_party/progress/progress.hpp" @@ -62,15 +63,9 @@ int main(int argc, char ** argv) // Read images names //--------------------------------------- - std::vector vec_fileNames; - { - std::ifstream in(stlplus::create_filespec(sMatchesDir, "lists", "txt").c_str()); - std::string sValue; - while(in>>sValue) - vec_fileNames.push_back(sValue); - in.close(); - } - if (vec_fileNames.empty()) { + std::vector vec_fileNames; + if (!SfMIO::loadImageList( vec_fileNames, + stlplus::create_filespec(sMatchesDir, "lists", "txt"),false)) { std::cerr << "\nEmpty input image list" << std::endl; return EXIT_FAILURE; } @@ -80,20 +75,6 @@ int main(int argc, char ** argv) // For each image, export visually the keypoints // ------------ - //-- - //- Preprocess the images size - Image image; - std::map< std::string, std::pair > map_imageSize; - for (std::vector::const_iterator iterFilename = vec_fileNames.begin(); - iterFilename != vec_fileNames.end(); - ++iterFilename) - { - ReadImage( stlplus::create_filespec(sImaDirectory,*iterFilename).c_str() , &image); - map_imageSize.insert( - std::make_pair(*iterFilename, - std::make_pair(image.Width(), image.Height()))); - } - stlplus::folder_create(sOutDir); std::cout << "\n Export extracted keypoints for all images" << std::endl; C_Progress_display my_progress_bar( vec_fileNames.size() ); @@ -105,7 +86,11 @@ int main(int argc, char ** argv) (std::vector::const_iterator)vec_fileNames.begin(), iterFilename); - const std::pair dimImage = map_imageSize.find(*iterFilename)->second; + //-- + //- Read the image size + Image image; + ReadImage( stlplus::create_filespec(sImaDirectory,*iterFilename).c_str() , &image); + const std::pair dimImage (image.Width(),image.Height()); svgDrawer svgStream( dimImage.first, dimImage.second); svgStream.drawImage(stlplus::create_filespec(sImaDirectory,*iterFilename), diff --git a/src/software/SfM/main_exportMatches.cpp b/src/software/SfM/main_exportMatches.cpp index bf19b1e3ac..8e078aa58f 100644 --- a/src/software/SfM/main_exportMatches.cpp +++ b/src/software/SfM/main_exportMatches.cpp @@ -10,6 +10,7 @@ #include "openMVG/image/image.hpp" #include "openMVG/features/features.hpp" +#include "software/SfM/SfMIOHelper.hpp" #include "third_party/cmdLine/cmdLine.h" #include "third_party/stlplus3/filesystemSimplified/file_system.hpp" #include "third_party/progress/progress.hpp" @@ -65,15 +66,9 @@ int main(int argc, char ** argv) // Read images names //--------------------------------------- - std::vector vec_fileNames; - { - std::ifstream in(stlplus::create_filespec(sMatchesDir, "lists", "txt").c_str()); - std::string sValue; - while(in>>sValue) - vec_fileNames.push_back(sValue); - in.close(); - } - if (vec_fileNames.empty()) { + std::vector vec_fileNames; + if (!SfMIO::loadImageList( vec_fileNames, + stlplus::create_filespec(sMatchesDir, "lists", "txt"),false)) { std::cerr << "\nEmpty input image list" << std::endl; return EXIT_FAILURE; } diff --git a/src/software/SfM/main_exportTracks.cpp b/src/software/SfM/main_exportTracks.cpp index 956444ee05..21dac7caba 100644 --- a/src/software/SfM/main_exportTracks.cpp +++ b/src/software/SfM/main_exportTracks.cpp @@ -11,6 +11,7 @@ #include "openMVG/features/features.hpp" #include "openMVG/tracks/tracks.hpp" +#include "software/SfM/SfMIOHelper.hpp" #include "third_party/cmdLine/cmdLine.h" #include "third_party/stlplus3/filesystemSimplified/file_system.hpp" #include "third_party/progress/progress.hpp" @@ -67,15 +68,9 @@ int main(int argc, char ** argv) // Read images names //--------------------------------------- - std::vector vec_fileNames; - { - std::ifstream in(stlplus::create_filespec(sMatchesDir, "lists", "txt").c_str()); - std::string sValue; - while(in>>sValue) - vec_fileNames.push_back(sValue); - in.close(); - } - if (vec_fileNames.empty()) { + std::vector vec_fileNames; + if (!SfMIO::loadImageList( vec_fileNames, + stlplus::create_filespec(sMatchesDir, "lists", "txt"),false)) { std::cerr << "\nEmpty input image list" << std::endl; return EXIT_FAILURE; } From 43bcb4b7c24b570ac596a8a273583e01bf4df63c Mon Sep 17 00:00:00 2001 From: pmoulon Date: Thu, 30 Jan 2014 20:33:50 +0100 Subject: [PATCH 39/68] Initial revision of the matching_image_collection module.#73 --- ...llection_E_ACRobust.hpp => E_ACRobust.hpp} | 7 +- ...llection_F_ACRobust.hpp => F_ACRobust.hpp} | 7 +- ...eometricFilter.hpp => GeometricFilter.hpp} | 9 +- .../matching_image_collection/H_ACRobust.hpp | 65 ++++++ ...ImageCollectionMatcher.hpp => Matcher.hpp} | 20 +- ...llInMemory.hpp => Matcher_AllInMemory.hpp} | 30 +-- src/software/SfM/main_computeMatches.cpp | 212 +++++++++++++----- .../main_computeMatchesEssential.cpp | 16 +- 8 files changed, 262 insertions(+), 104 deletions(-) rename src/openMVG/matching_image_collection/{ImageCollection_E_ACRobust.hpp => E_ACRobust.hpp} (98%) rename src/openMVG/matching_image_collection/{ImageCollection_F_ACRobust.hpp => F_ACRobust.hpp} (97%) rename src/openMVG/matching_image_collection/{ImageCollectionGeometricFilter.hpp => GeometricFilter.hpp} (94%) create mode 100644 src/openMVG/matching_image_collection/H_ACRobust.hpp rename src/openMVG/matching_image_collection/{ImageCollectionMatcher.hpp => Matcher.hpp} (58%) rename src/openMVG/matching_image_collection/{ImageCollectionMatcher_AllInMemory.hpp => Matcher_AllInMemory.hpp} (90%) diff --git a/src/openMVG/matching_image_collection/ImageCollection_E_ACRobust.hpp b/src/openMVG/matching_image_collection/E_ACRobust.hpp similarity index 98% rename from src/openMVG/matching_image_collection/ImageCollection_E_ACRobust.hpp rename to src/openMVG/matching_image_collection/E_ACRobust.hpp index e01365f345..31d2de578c 100644 --- a/src/openMVG/matching_image_collection/ImageCollection_E_ACRobust.hpp +++ b/src/openMVG/matching_image_collection/E_ACRobust.hpp @@ -10,12 +10,11 @@ #include "openMVG/multiview/solver_essential_kernel.hpp" #include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" #include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp" +#include -using namespace openMVG; +namespace openMVG { using namespace openMVG::robust; -#include - //-- A contrario Functor to filter putative corresponding points //-- thanks estimation of the essential matrix. //- Suppose that all image have the same K matrix @@ -66,3 +65,5 @@ struct GeometricFilter_EMatrix_AC size_t m_stIteration; //maximal number of used iterations Mat3 m_K; // the considered intrinsic matrix }; + +}; // namespace openMVG diff --git a/src/openMVG/matching_image_collection/ImageCollection_F_ACRobust.hpp b/src/openMVG/matching_image_collection/F_ACRobust.hpp similarity index 97% rename from src/openMVG/matching_image_collection/ImageCollection_F_ACRobust.hpp rename to src/openMVG/matching_image_collection/F_ACRobust.hpp index 5f07e58903..236e853cd1 100644 --- a/src/openMVG/matching_image_collection/ImageCollection_F_ACRobust.hpp +++ b/src/openMVG/matching_image_collection/F_ACRobust.hpp @@ -10,12 +10,11 @@ #include "openMVG/multiview/solver_fundamental_kernel.hpp" #include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" #include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp" +#include -using namespace openMVG; +namespace openMVG { using namespace openMVG::robust; -#include - //-- A contrario Functor to filter putative corresponding points struct GeometricFilter_FMatrix_AC { @@ -60,3 +59,5 @@ struct GeometricFilter_FMatrix_AC double m_dPrecision; //upper_bound of the precision size_t m_stIteration; //maximal number of iteration used }; + +}; // namespace openMVG diff --git a/src/openMVG/matching_image_collection/ImageCollectionGeometricFilter.hpp b/src/openMVG/matching_image_collection/GeometricFilter.hpp similarity index 94% rename from src/openMVG/matching_image_collection/ImageCollectionGeometricFilter.hpp rename to src/openMVG/matching_image_collection/GeometricFilter.hpp index 33472ec5cd..20bfde5192 100644 --- a/src/openMVG/matching_image_collection/ImageCollectionGeometricFilter.hpp +++ b/src/openMVG/matching_image_collection/GeometricFilter.hpp @@ -17,7 +17,7 @@ using namespace openMVG; #include #include -template +template class ImageCollectionGeometricFilter { public: @@ -25,14 +25,14 @@ class ImageCollectionGeometricFilter { } - /// Load all features and descriptors in memory + /// Load all features in memory bool loadData( const std::vector & vec_fileNames, // input filenames const std::string & sMatchDir) // where the data are saved { bool bOk = true; for (size_t j = 0; j < vec_fileNames.size(); ++j) { - // Load descriptor of Jnth image + // Load features of Jnth image const std::string sFeatJ = stlplus::create_filespec(sMatchDir, stlplus::basename_part(vec_fileNames[j]), "feat"); bOk &= loadFeatsFromFile(sFeatJ, map_Feat[j]); @@ -40,7 +40,8 @@ class ImageCollectionGeometricFilter return bOk; } - /// Filter all putatives correspondances according the Template filter + /// Filter all putative correspondences according the templated geometric filter + template void Filter( const GeometricFilterT & geometricFilter, IndexedMatchPerPair & map_PutativesMatchesPair, // putative correspondences to filter diff --git a/src/openMVG/matching_image_collection/H_ACRobust.hpp b/src/openMVG/matching_image_collection/H_ACRobust.hpp new file mode 100644 index 0000000000..2ab21e4c02 --- /dev/null +++ b/src/openMVG/matching_image_collection/H_ACRobust.hpp @@ -0,0 +1,65 @@ + +// Copyright (c) 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +#pragma once + +#include "openMVG/multiview/solver_homography_kernel.hpp" +#include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" +#include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp" +#include + +namespace openMVG { +using namespace openMVG::robust; + +//-- A contrario Functor to filter putative corresponding points +//-- thanks estimation of the homography matrix. +struct GeometricFilter_HMatrix_AC +{ + GeometricFilter_HMatrix_AC( + double dPrecision = std::numeric_limits::infinity(), + size_t iteration = 4096) + : m_dPrecision(dPrecision), m_stIteration(iteration) {}; + + /// Robust fitting of the HOMOGRAPHY matrix + void Fit( + const Mat & xA, + const std::pair & imgSizeA, + const Mat & xB, + const std::pair & imgSizeB, + std::vector & vec_inliers) const + { + vec_inliers.resize(0); + + // Define the AContrario adapted Homography matrix solver + typedef ACKernelAdaptor< + openMVG::homography::kernel::FourPointSolver, + openMVG::homography::kernel::AsymmetricError, + UnnormalizerI, + Mat3> + KernelType; + + KernelType kernel( + xA, imgSizeA.first, imgSizeA.second, + xB, imgSizeB.first, imgSizeB.second, + false); // configure as point to point error model. + + // Robustly estimate the Homography matrix with A Contrario ransac + Mat3 H; + double upper_bound_precision = m_dPrecision; + std::pair ACRansacOut = + ACRANSAC(kernel, vec_inliers, m_stIteration, &H, upper_bound_precision); + + if (vec_inliers.size() < KernelType::MINIMUM_SAMPLES *2.5) { + vec_inliers.clear(); + } + } + + double m_dPrecision; //upper_bound of the precision + size_t m_stIteration; //maximal number of used iterations +}; + +}; // namespace openMVG diff --git a/src/openMVG/matching_image_collection/ImageCollectionMatcher.hpp b/src/openMVG/matching_image_collection/Matcher.hpp similarity index 58% rename from src/openMVG/matching_image_collection/ImageCollectionMatcher.hpp rename to src/openMVG/matching_image_collection/Matcher.hpp index 0d034b44fc..2b2fec612e 100644 --- a/src/openMVG/matching_image_collection/ImageCollectionMatcher.hpp +++ b/src/openMVG/matching_image_collection/Matcher.hpp @@ -1,5 +1,5 @@ -// Copyright (c) 2012, 2013 Pierre MOULON. +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. // 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 @@ -8,29 +8,29 @@ #pragma once #include "openMVG/matching/indMatch.hpp" - -using namespace openMVG::matching; - #include #include #include +namespace openMVG { + /// The structure used to store corresponding point index per images pairs -typedef std::map< std::pair, std::vector > IndexedMatchPerPair; +typedef std::map< std::pair, std::vector > IndexedMatchPerPair; /// Implementation of an Image Collection Matcher /// Compute putative matches between a collection of pictures -class ImageCollectionMatcher +class Matcher { public: - ImageCollectionMatcher() {}; + Matcher() {}; - virtual ~ImageCollectionMatcher() {}; + virtual ~Matcher() {}; /// Build point indexes correspondences lists between images ids virtual void Match( - const std::vector & vec_fileNames, // input filenames, - IndexedMatchPerPair & map_putatives_matches // the parwise photometric corresponding points + const std::vector & vec_filenames, + IndexedMatchPerPair & map_putatives_matches // the output pairwise photometric corresponding points )const = 0; }; +}; // namespace openMVG diff --git a/src/openMVG/matching_image_collection/ImageCollectionMatcher_AllInMemory.hpp b/src/openMVG/matching_image_collection/Matcher_AllInMemory.hpp similarity index 90% rename from src/openMVG/matching_image_collection/ImageCollectionMatcher_AllInMemory.hpp rename to src/openMVG/matching_image_collection/Matcher_AllInMemory.hpp index bb7cb6513a..991917d3c9 100644 --- a/src/openMVG/matching_image_collection/ImageCollectionMatcher_AllInMemory.hpp +++ b/src/openMVG/matching_image_collection/Matcher_AllInMemory.hpp @@ -1,5 +1,5 @@ -// Copyright (c) 2012, 2013 Pierre MOULON. +// Copyright (c) 2012, 2013, 2014 Pierre MOULON. // 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 @@ -8,26 +8,25 @@ #pragma once -#include "software/SfM/ImageCollectionMatcher.hpp" #include "openMVG/features/features.hpp" #include "openMVG/matching/indMatchDecoratorXY.hpp" #include "openMVG/matching/matching_filters.hpp" - - -using namespace openMVG; +#include "openMVG/matching_image_collection/Matcher.hpp" #include "third_party/stlplus3/filesystemSimplified/file_system.hpp" #include "third_party/progress/progress.hpp" +namespace openMVG { + +using namespace openMVG::matching; + /// Implementation of an Image Collection Matcher /// Compute putative matches between a collection of pictures -/// Suppose a symmetric matching results : Will compare the -/// upper matrix index for image matching. -/// For descriptor matching between two image indexes : -/// The distance ratio of the 2 neighbours points is used -/// to discard spurious correspondences. +/// Spurious correspondences are discarded by using the +/// a threshold over the distance ratio of the 2 neighbours points. +/// template -class ImageCollectionMatcher_AllInMemory : public ImageCollectionMatcher +class Matcher_AllInMemory : public Matcher { // Alias to internal stored Feature and Descriptor type typedef typename KeypointSetT::FeatureT FeatureT; @@ -37,7 +36,9 @@ class ImageCollectionMatcher_AllInMemory : public ImageCollectionMatcher typedef typename DescriptorT::bin_type DescBin_typeT; public: - ImageCollectionMatcher_AllInMemory(float distRatio) :ImageCollectionMatcher(), fDistRatio(distRatio) + Matcher_AllInMemory(float distRatio): + Matcher(), + fDistRatio(distRatio) { } @@ -55,12 +56,10 @@ class ImageCollectionMatcher_AllInMemory : public ImageCollectionMatcher // Load descriptor of Jnth image const std::string sFeatJ = stlplus::create_filespec(sMatchDir, stlplus::basename_part(vec_fileNames[j]), "feat"); - const std::string sDescJ = stlplus::create_filespec(sMatchDir, stlplus::basename_part(vec_fileNames[j]), "desc"); - bOk &= loadFeatsFromFile(sFeatJ, map_Feat[j]); - KeypointSetT kpSetI; + bOk &= loadFeatsFromFile(sFeatJ, map_Feat[j]); bOk &= loadDescsFromBinFile(sDescJ, map_Desc[j]); } return bOk; @@ -157,3 +156,4 @@ class ImageCollectionMatcher_AllInMemory : public ImageCollectionMatcher float fDistRatio; }; +}; // namespace openMVG diff --git a/src/software/SfM/main_computeMatches.cpp b/src/software/SfM/main_computeMatches.cpp index b18971dc3c..6cf7cc617a 100644 --- a/src/software/SfM/main_computeMatches.cpp +++ b/src/software/SfM/main_computeMatches.cpp @@ -10,9 +10,11 @@ #include "openMVG/features/features.hpp" /// Generic Image Collection image matching -#include "software/SfM/ImageCollectionMatcher_AllInMemory.hpp" -#include "software/SfM/ImageCollectionGeometricFilter.hpp" -#include "software/SfM/ImageCollection_F_ACRobust.hpp" +#include "openMVG/matching_image_collection/Matcher_AllInMemory.hpp" +#include "openMVG/matching_image_collection/GeometricFilter.hpp" +#include "openMVG/matching_image_collection/F_ACRobust.hpp" +#include "openMVG/matching_image_collection/E_ACRobust.hpp" +#include "openMVG/matching_image_collection/H_ACRobust.hpp" #include "software/SfM/pairwiseAdjacencyDisplay.hpp" #include "software/SfM/SfMIOHelper.hpp" #include "openMVG/matching/matcher_brute_force.hpp" @@ -38,12 +40,28 @@ using namespace openMVG::matching; using namespace openMVG::robust; using namespace std; +enum eGeometricModel +{ + FUNDAMENTAL_MATRIX = 0, + ESSENTIAL_MATRIX = 1, + HOMOGRAPHY_MATRIX = 2 +}; + +// Equality functor to count the number of similar K matrices in the essential matrix case. +bool testIntrinsicsEquality( + SfMIO::IntrinsicCameraInfo const &ci1, + SfMIO::IntrinsicCameraInfo const &ci2) +{ + return ci1.m_K == ci2.m_K; +} + int main(int argc, char **argv) { CmdLine cmd; std::string sImaDirectory; std::string sOutDir = ""; + std::string sGeometricModel = "f"; float fDistRatio = .6f; bool bOctMinus1 = false; float dPeakThreshold = 0.04f; @@ -53,17 +71,20 @@ int main(int argc, char **argv) cmd.add( make_option('r', fDistRatio, "distratio") ); cmd.add( make_option('s', bOctMinus1, "octminus1") ); cmd.add( make_option('p', dPeakThreshold, "peakThreshold") ); + cmd.add( make_option('g', sGeometricModel, "geometricModel") ); try { if (argc == 1) throw std::string("Invalid command line parameter."); cmd.process(argc, argv); } catch(const std::string& s) { - std::cerr << "Usage: " << argv[0] << ' ' - << "[-i|--imadir path] " - << "[-o|--outdir path] " - << "[-r|--distratio 0.6] " - << "[-s|--octminus1 0 or 1] " - << "[-p|--peakThreshold 0.04 -> 0.01] " + std::cerr << "Usage: " << argv[0] << '\n' + << "[-i|--imadir path] \n" + << "[-o|--outdir path] \n" + << "\n[Optional]\n" + << "[-r|--distratio 0.6] \n" + << "[-s|--octminus1 0 or 1] \n" + << "[-p|--peakThreshold 0.04 -> 0.01] \n" + << "[-g]--geometricModel f, e or h]" << std::endl; std::cerr << s << std::endl; @@ -76,18 +97,40 @@ int main(int argc, char **argv) << "--outdir " << sOutDir << std::endl << "--distratio " << fDistRatio << std::endl << "--octminus1 " << bOctMinus1 << std::endl - << "--peakThreshold " << dPeakThreshold << std::endl; + << "--peakThreshold " << dPeakThreshold << std::endl + << "--geometricModel " << sGeometricModel << std::endl; if (sOutDir.empty()) { std::cerr << "\nIt is an invalid output directory" << std::endl; return EXIT_FAILURE; } + eGeometricModel eGeometricModelToCompute = FUNDAMENTAL_MATRIX; + std::string sGeometricMatchesFilename = ""; + switch(sGeometricModel[0]) + { + case 'f': case 'F': + eGeometricModelToCompute = FUNDAMENTAL_MATRIX; + sGeometricMatchesFilename = "matches.f.txt"; + break; + case 'e': case 'E': + eGeometricModelToCompute = ESSENTIAL_MATRIX; + sGeometricMatchesFilename = "matches.e.txt"; + break; + case 'h': case 'H': + eGeometricModelToCompute = HOMOGRAPHY_MATRIX; + sGeometricMatchesFilename = "matches.h.txt"; + break; + default: + std::cerr << "Unknown geometric model" << std::endl; + return EXIT_FAILURE; + } + // ----------------------------- // a. List images - // b. Compute features and descriptor - // c. Compute putatives descriptor matches - // d. Geometric filtering of putatives matches + // b. Compute features and descriptors + // c. Compute putative descriptor matches + // d. Geometric filtering of putative matches // e. Export some statistics // ----------------------------- @@ -98,10 +141,8 @@ int main(int argc, char **argv) //--------------------------------------- // a. List images //--------------------------------------- - std::string sListsFile = stlplus::create_filespec( sOutDir, - "lists.txt" ).c_str(); - if (!stlplus::is_file(sListsFile) ) - { + std::string sListsFile = stlplus::create_filespec(sOutDir, "lists.txt" ); + if (!stlplus::is_file(sListsFile)) { std::cerr << std::endl << "The input file \""<< sListsFile << "\" is missing" << std::endl; return false; @@ -117,11 +158,33 @@ int main(int argc, char **argv) return false; } + if (eGeometricModelToCompute == ESSENTIAL_MATRIX) + { + //-- In the case of the essential matrix we check if only one K matrix is present. + //-- Due to the fact that the generic framework allows only one K matrix for the + // robust essential matrix estimation in image collection. + std::vector::iterator iterF = + std::unique(vec_focalGroup.begin(), vec_focalGroup.end(), testIntrinsicsEquality); + vec_focalGroup.resize( std::distance(vec_focalGroup.begin(), iterF) ); + if (vec_focalGroup.size() == 1) { + // Set all the intrinsic ID to 0 + for (size_t i = 0; i < vec_camImageName.size(); ++i) + vec_camImageName[i].m_intrinsicId = 0; + } + else { + std::cerr << "There is more than one focal group in the lists.txt file." << std::endl + << "Only one focal group is supported for the image collection robust essential matrix estimation." << std::endl; + return EXIT_FAILURE; + } + } + + //-- Two alias to ease access to image filenames and image sizes std::vector vec_fileNames; std::vector > vec_imagesSize; - for ( std::vector::const_iterator iter_camInfo = vec_camImageName.begin(); - iter_camInfo != vec_camImageName.end(); - iter_camInfo++ ) + for ( std::vector::const_iterator + iter_camInfo = vec_camImageName.begin(); + iter_camInfo != vec_camImageName.end(); + iter_camInfo++ ) { vec_imagesSize.push_back( std::make_pair( vec_focalGroup[iter_camInfo->m_intrinsicId].m_w, vec_focalGroup[iter_camInfo->m_intrinsicId].m_h ) ); @@ -150,49 +213,35 @@ int main(int argc, char **argv) C_Progress_display my_progress_bar( vec_fileNames.size() ); for(size_t i=0; i < vec_fileNames.size(); ++i) { - KeypointSetT kpSet; std::string sFeat = stlplus::create_filespec(sOutDir, stlplus::basename_part(vec_fileNames[i]), "feat"); std::string sDesc = stlplus::create_filespec(sOutDir, stlplus::basename_part(vec_fileNames[i]), "desc"); - //Test if descriptors and features was already computed - if (stlplus::file_exists(sFeat) && stlplus::file_exists(sDesc)) { - - if (ReadImage(vec_fileNames[i].c_str(), &imageRGB)) { - vec_imagesSize[i] = make_pair(imageRGB.Width(), imageRGB.Height()); - } - else { - ReadImage(vec_fileNames[i].c_str(), &imageGray); - vec_imagesSize[i] = make_pair(imageGray.Width(), imageGray.Height()); - } - } - else { //Not already computed, so compute and save + //If descriptors or features file are missing, compute them + if (!stlplus::file_exists(sFeat) || !stlplus::file_exists(sDesc)) { if (ReadImage(vec_fileNames[i].c_str(), &imageRGB)) { Rgb2Gray(imageRGB, &imageGray); } - else{ + else { ReadImage(vec_fileNames[i].c_str(), &imageGray); } - // Compute features and descriptors and export them to file + // Compute features and descriptors and export them to files + KeypointSetT kpSet; SIFTDetector(imageGray, kpSet.features(), kpSet.descriptors(), bOctMinus1, true, dPeakThreshold); - kpSet.saveToBinFile(sFeat, sDesc); - vec_imagesSize[i] = make_pair(imageGray.Width(), imageRGB.Height()); } ++my_progress_bar; } } - - //--------------------------------------- - // c. Compute putatives descriptor matches + // c. Compute putative descriptor matches // - L2 descriptor matching // - Keep correspondences only if NearestNeighbor ratio is ok //--------------------------------------- @@ -201,7 +250,7 @@ int main(int argc, char **argv) // ANN matcher could be defined as follow: typedef flann::L2 MetricT; typedef ArrayMatcher_Kdtree_Flann MatcherT; - // Brute force matcher is defined as following: + // Brute force matcher can be defined as following: //typedef L2_Vectorized MetricT; //typedef ArrayMatcherBruteForce MatcherT; @@ -213,7 +262,7 @@ int main(int argc, char **argv) } else // Compute the putatives matches { - ImageCollectionMatcher_AllInMemory collectionMatcher(fDistRatio); + Matcher_AllInMemory collectionMatcher(fDistRatio); if (collectionMatcher.loadData(vec_fileNames, sOutDir)) { std::cout << std::endl << "PUTATIVE MATCHES" << std::endl; @@ -234,38 +283,83 @@ int main(int argc, char **argv) //--------------------------------------- - // d. Geometric filtering of putatives matches - // - AContrario Estimation of the Fundamental matrix - // - Use a upper bound for the plausible F matrix - // acontrario estimated threshold + // d. Geometric filtering of putative matches + // - AContrario Estimation of the desired geometric model + // - Use an upper bound for the a contrario estimated threshold //--------------------------------------- - IndexedMatchPerPair map_GeometricMatches_F; + IndexedMatchPerPair map_GeometricMatches; - GeometricFilter_FMatrix_AC geomFilter_F_AC(4.0); - ImageCollectionGeometricFilter collectionGeomFilter; + ImageCollectionGeometricFilter collectionGeomFilter; + const double maxResidualError = 4.0; if (collectionGeomFilter.loadData(vec_fileNames, sOutDir)) { std::cout << std::endl << " - GEOMETRIC FILTERING - " << std::endl; - collectionGeomFilter.Filter( - geomFilter_F_AC, - map_PutativesMatches, - map_GeometricMatches_F, - vec_imagesSize); + switch (eGeometricModelToCompute) + { + case FUNDAMENTAL_MATRIX: + { + collectionGeomFilter.Filter( + GeometricFilter_FMatrix_AC(maxResidualError), + map_PutativesMatches, + map_GeometricMatches, + vec_imagesSize); + } + break; + case ESSENTIAL_MATRIX: + { + collectionGeomFilter.Filter( + GeometricFilter_EMatrix_AC(vec_focalGroup[0].m_K, maxResidualError), + map_PutativesMatches, + map_GeometricMatches, + vec_imagesSize); + + //-- Perform an additional check to remove pairs with poor overlap + std::vector vec_toRemove; + for (IndexedMatchPerPair::const_iterator iterMap = map_GeometricMatches.begin(); + iterMap != map_GeometricMatches.end(); ++iterMap) + { + size_t putativePhotometricCount = map_PutativesMatches.find(iterMap->first)->second.size(); + size_t putativeGeometricCount = iterMap->second.size(); + float ratio = putativeGeometricCount / (float)putativePhotometricCount; + if (putativeGeometricCount < 50 || ratio < .3f) { + // the pair will be removed + vec_toRemove.push_back(iterMap->first); + } + } + //-- remove discarded pairs + for (std::vector::const_iterator + iter = vec_toRemove.begin(); iter != vec_toRemove.end(); ++iter) + { + map_GeometricMatches.erase(*iter); + } + } + break; + case HOMOGRAPHY_MATRIX: + { + + collectionGeomFilter.Filter( + GeometricFilter_HMatrix_AC(maxResidualError), + map_PutativesMatches, + map_GeometricMatches, + vec_imagesSize); + } + break; + } //--------------------------------------- //-- Export geometric filtered matches //--------------------------------------- - std::ofstream file (string(sOutDir + "/matches.f.txt").c_str()); + std::ofstream file (string(sOutDir + "/" + sGeometricMatchesFilename).c_str()); if (file.is_open()) - PairedIndMatchToStream(map_GeometricMatches_F, file); + PairedIndMatchToStream(map_GeometricMatches, file); file.close(); //-- export Adjacency matrix - std::cout << "\n Export Adjacency Matrix of the pairwise's Epipolar matches" + std::cout << "\n Export Adjacency Matrix of the pairwise's geometric matches" << std::endl; PairWiseMatchingToAdjacencyMatrixSVG(vec_fileNames.size(), - map_GeometricMatches_F, - stlplus::create_filespec(sOutDir, "EpipolarAdjacencyMatrix", "svg")); + map_GeometricMatches, + stlplus::create_filespec(sOutDir, "GeometricAdjacencyMatrix", "svg")); } return EXIT_SUCCESS; } diff --git a/src/software/globalSfM/main_computeMatchesEssential.cpp b/src/software/globalSfM/main_computeMatchesEssential.cpp index 9c71ce99ae..1c62053271 100644 --- a/src/software/globalSfM/main_computeMatchesEssential.cpp +++ b/src/software/globalSfM/main_computeMatchesEssential.cpp @@ -10,9 +10,9 @@ #include "openMVG/features/features.hpp" /// Generic Image Collection image matching -#include "software/SfM/ImageCollectionMatcher_AllInMemory.hpp" -#include "software/SfM/ImageCollectionGeometricFilter.hpp" -#include "software/globalSfM/ImageCollection_E_ACRobust.hpp" +#include "openMVG/matching_image_collection/Matcher_AllInMemory.hpp" +#include "openMVG/matching_image_collection/GeometricFilter.hpp" +#include "openMVG/matching_image_collection/E_ACRobust.hpp" #include "software/SfM/pairwiseAdjacencyDisplay.hpp" #include "software/SfM/SfMIOHelper.hpp" #include "openMVG/matching/matcher_brute_force.hpp" @@ -142,8 +142,6 @@ int main(int argc, char **argv) return EXIT_FAILURE; } -// Using UNIQUE with a special functor and distance == (1) ? - std::vector vec_fileNames; std::vector > vec_imagesSize; for ( std::vector::const_iterator iter_camInfo = vec_camImageName.begin(); @@ -238,7 +236,7 @@ int main(int argc, char **argv) } else // Compute the putatives matches { - ImageCollectionMatcher_AllInMemory collectionMatcher(fDistRatio); + Matcher_AllInMemory collectionMatcher(fDistRatio); if (collectionMatcher.loadData(vec_fileNames, sOutDir)) { std::cout << std::endl << "PUTATIVE MATCHES" << std::endl; @@ -265,14 +263,12 @@ int main(int argc, char **argv) // acontrario estimated threshold //--------------------------------------- IndexedMatchPerPair map_GeometricMatches_E; - - GeometricFilter_EMatrix_AC geomFilter_E_AC(vec_focalGroup[0].m_K, 4.0); - ImageCollectionGeometricFilter collectionGeomFilter; + ImageCollectionGeometricFilter collectionGeomFilter; if (collectionGeomFilter.loadData(vec_fileNames, sOutDir)) { std::cout << std::endl << " - GEOMETRIC FILTERING - " << std::endl; collectionGeomFilter.Filter( - geomFilter_E_AC, + GeometricFilter_EMatrix_AC(vec_focalGroup[0].m_K, 4.0), map_PutativesMatches, map_GeometricMatches_E, vec_imagesSize); From de5bb5e6fa1307ee9b15e66e32872d699a83763c Mon Sep 17 00:00:00 2001 From: pmoulon Date: Thu, 30 Jan 2014 20:34:11 +0100 Subject: [PATCH 40/68] numerical precision fix. --- src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp b/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp index 65ceaab305..afa4e18f71 100644 --- a/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp +++ b/src/openMVG/robust_estimation/robust_estimator_ACRansac.hpp @@ -95,7 +95,7 @@ static ErrorIndex bestNFA( ErrorIndex bestIndex(std::numeric_limits::infinity(), startIndex); const size_t n = e.size(); for(size_t k=startIndex+1; k<=n && e[k-1].first<=maxThreshold; ++k) { - double logalpha = logalpha0 + multError *log10(e[k-1].first + std::numeric_limits::min()); + double logalpha = logalpha0 + multError * log10(e[k-1].first + std::numeric_limits::min()); ErrorIndex index(loge0 + logalpha * (double)(k-startIndex) + logc_n[k] + From 28b76a1c95679681d060781f8a2b5f0ae5b94555 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Thu, 30 Jan 2014 23:13:02 +0100 Subject: [PATCH 41/68] Initial implementation of the paper 'Global Multiple-View Color Consistency'. #74 --- src/openMVG/CMakeLists.txt | 1 + .../color_harmonization/CMakeLists.txt | 6 + .../global_quantile_gain_offset_alignment.hpp | 257 ++++++++ ...al_quantile_gain_offset_alignment_test.cpp | 208 ++++++ .../selection_VLDSegment.hpp | 123 ++++ .../selection_fullFrame.hpp | 48 ++ .../selection_interface.hpp | 79 +++ .../selection_matchedPoints.hpp | 73 +++ src/openMVG/image/image_converter.hpp | 55 ++ src/openMVG/image/image_drawing.hpp | 36 ++ src/openMVG/image/pixel_types.hpp | 1 + src/software/CMakeLists.txt | 1 + src/software/colorHarmonize/CMakeLists.txt | 18 + .../colorHarmonizeEngineGlobal.cpp | 606 ++++++++++++++++++ .../colorHarmonizeEngineGlobal.hpp | 68 ++ .../colorHarmonize/main_ColHarmonize.cpp | 98 +++ 16 files changed, 1678 insertions(+) create mode 100644 src/openMVG/color_harmonization/CMakeLists.txt create mode 100644 src/openMVG/color_harmonization/global_quantile_gain_offset_alignment.hpp create mode 100644 src/openMVG/color_harmonization/global_quantile_gain_offset_alignment_test.cpp create mode 100644 src/openMVG/color_harmonization/selection_VLDSegment.hpp create mode 100644 src/openMVG/color_harmonization/selection_fullFrame.hpp create mode 100644 src/openMVG/color_harmonization/selection_interface.hpp create mode 100644 src/openMVG/color_harmonization/selection_matchedPoints.hpp create mode 100644 src/software/colorHarmonize/CMakeLists.txt create mode 100644 src/software/colorHarmonize/colorHarmonizeEngineGlobal.cpp create mode 100644 src/software/colorHarmonize/colorHarmonizeEngineGlobal.hpp create mode 100644 src/software/colorHarmonize/main_ColHarmonize.cpp diff --git a/src/openMVG/CMakeLists.txt b/src/openMVG/CMakeLists.txt index 29a9308729..167ed59c35 100644 --- a/src/openMVG/CMakeLists.txt +++ b/src/openMVG/CMakeLists.txt @@ -11,4 +11,5 @@ ADD_SUBDIRECTORY(numeric) ADD_SUBDIRECTORY(robust_estimation) ADD_SUBDIRECTORY(split) ADD_SUBDIRECTORY(tracks) +ADD_SUBDIRECTORY(color_harmonization) diff --git a/src/openMVG/color_harmonization/CMakeLists.txt b/src/openMVG/color_harmonization/CMakeLists.txt new file mode 100644 index 0000000000..4e13b4aa44 --- /dev/null +++ b/src/openMVG/color_harmonization/CMakeLists.txt @@ -0,0 +1,6 @@ + +UNIT_TEST( + openMVG + global_quantile_gain_offset_alignment + "openMVG_image;openMVG_linearProgramming") + diff --git a/src/openMVG/color_harmonization/global_quantile_gain_offset_alignment.hpp b/src/openMVG/color_harmonization/global_quantile_gain_offset_alignment.hpp new file mode 100644 index 0000000000..b89713b591 --- /dev/null +++ b/src/openMVG/color_harmonization/global_quantile_gain_offset_alignment.hpp @@ -0,0 +1,257 @@ + +// Copyright (c) 2013, 2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + +//------------------ +//-- Bibliography -- +//------------------ +//- [1] "Global Multiple-View Color Consistency." +//- Authors: Pierre MOULON, Bruno DUISIT and Pascal MONASSE. +//- Date: November 2013. +//- Conference: CVMP. + +//-- Linear programming +#include "openMVG/linearProgramming/linearProgrammingInterface.hpp" +#include "openMVG/linearProgramming/linearProgrammingOSI_X.hpp" +#include "openMVG/linearProgramming/linearProgrammingMOSEK.hpp" +#include "openMVG/linearProgramming/bisectionLP.hpp" + +namespace openMVG { +namespace lInfinity { + +struct relativeColorHistogramEdge +{ + size_t I,J; + std::vector histoI, histoJ; + + relativeColorHistogramEdge() {} + + relativeColorHistogramEdge( + size_t i, size_t j, + const std::vector & histogramI, + const std::vector & histogramJ): + I(i), J(j), + histoI(histogramI), + histoJ(histogramJ) + { } +}; + +namespace histogram { + +// Normalize a distribution function +template +static void normalizeHisto(const std::vector & vec_df, std::vector & vec_normalized_df) +{ + double totalCount = static_cast(std::accumulate(vec_df.begin(), vec_df.end(), 0)); + vec_normalized_df.resize(vec_df.size(), 0.0); + for(size_t i=0; i +static void cdf(const std::vector & vec_df, std::vector & vec_cdf) +{ + vec_cdf = vec_df; + for(size_t i=1; i & vec_relativeHistograms, + const std::vector & vec_indexToFix, + sRMat & A, Vec & C, + std::vector & vec_sign, + std::vector & vec_costs, + std::vector< std::pair > & vec_bounds) +{ + const size_t Nima = (size_t) nImage; + const size_t Nrelative = vec_relativeHistograms.size(); + +# define GVAR(i) (2*(i)) +# define OFFSETVAR(i) (2*(i)+1) +# define GAMMAVAR (2*Nima) + + const size_t nbQuantile = 10; + + const size_t Nconstraint = nbQuantile * Nrelative * 2; + const size_t NVar = 2 * Nima+ 1; + + A.resize(Nconstraint, NVar); + + C.resize(Nconstraint, 1); + C.fill(0.0); + vec_sign.resize(Nconstraint); + + // By default set free variable: + vec_bounds = std::vector< std::pair >(NVar); + fill( vec_bounds.begin(), vec_bounds.end(), + std::make_pair((double)-1e+30, (double)1e+30)); + + // Set gain as positive values + for (size_t i = 0; i < Nima; ++i) + { + vec_bounds[GVAR(i)].first = 0.0; + } + + //-- Fix the required image to known gain and offset value + for (std::vector::const_iterator iter = vec_indexToFix.begin(); + iter != vec_indexToFix.end(); ++iter) + { + vec_bounds[GVAR(*iter)] = std::make_pair(1.0, 1.0); // gain = 1.0 + vec_bounds[OFFSETVAR(*iter)] = std::make_pair(0.0, 0.0); // offset = 0 + } + + // Setup gamma >= 0 + vec_bounds[GAMMAVAR].first = 0.0; + + //-- Minimize gamma + vec_costs.resize(NVar); + std::fill(vec_costs.begin(), vec_costs.end(), 0.0); + vec_costs[GAMMAVAR] = 1.0; + //-- + + size_t rowPos = 0; + double incrementPourcentile = 1./(double) nbQuantile; + + for (size_t i = 0; i < Nrelative; ++i) + { + std::vector::const_iterator iter = vec_relativeHistograms.begin(); + std::advance(iter, i); + + const relativeColorHistogramEdge & edge = *iter; + + //-- compute the two cumulated and normalized histogram + + const std::vector< size_t > & vec_histoI = edge.histoI; + const std::vector< size_t > & vec_histoJ = edge.histoJ; + + const size_t nBuckets = vec_histoI.size(); + + // Normalize histogram + std::vector ndf_I(nBuckets), ndf_J(nBuckets); + histogram::normalizeHisto(vec_histoI, ndf_I); + histogram::normalizeHisto(vec_histoJ, ndf_J); + + // Compute cumulative distribution functions (cdf) + std::vector cdf_I(nBuckets), cdf_J(nBuckets); + histogram::cdf(ndf_I, cdf_I); + histogram::cdf(ndf_J, cdf_J); + + double currentPourcentile = 5./100.; + + //-- Compute pourcentile and their positions + std::vector vec_pourcentilePositionI, vec_pourcentilePositionJ; + vec_pourcentilePositionI.reserve(1.0/incrementPourcentile); + vec_pourcentilePositionJ.reserve(1.0/incrementPourcentile); + + std::vector::const_iterator cdf_I_IterBegin = cdf_I.begin(); + std::vector::const_iterator cdf_J_IterBegin = cdf_J.begin(); + while( currentPourcentile < 1.0) + { + std::vector::const_iterator iterFI = std::lower_bound(cdf_I.begin(), cdf_I.end(), currentPourcentile); + const size_t positionI = std::distance(cdf_I_IterBegin, iterFI); + + std::vector::const_iterator iterFJ = std::lower_bound(cdf_J.begin(), cdf_J.end(), currentPourcentile); + const size_t positionJ = std::distance(cdf_J_IterBegin, iterFJ); + + vec_pourcentilePositionI.push_back(positionI); + vec_pourcentilePositionJ.push_back(positionJ); + + currentPourcentile += incrementPourcentile; + } + + //-- Add the constraints: + // pos * ga + offa - pos * gb - offb <= gamma + // pos * ga + offa - pos * gb - offb >= - gamma + + for(size_t k = 0; k < vec_pourcentilePositionI.size(); ++k) + { + A.coeffRef(rowPos, GVAR(edge.I)) = vec_pourcentilePositionI[k]; + A.coeffRef(rowPos, OFFSETVAR(edge.I)) = 1.0; + + A.coeffRef(rowPos, GVAR(edge.J)) = - vec_pourcentilePositionJ[k]; + A.coeffRef(rowPos, OFFSETVAR(edge.J)) = - 1.0; + + // - gamma (side change) + A.coeffRef(rowPos, GAMMAVAR) = -1; + // <= gamma + vec_sign[rowPos] = linearProgramming::LP_Constraints::LP_LESS_OR_EQUAL; + C(rowPos) = 0; + ++rowPos; + + A.coeffRef(rowPos, GVAR(edge.I)) = vec_pourcentilePositionI[k]; + A.coeffRef(rowPos, OFFSETVAR(edge.I)) = 1.0; + + A.coeffRef(rowPos, GVAR(edge.J)) = - vec_pourcentilePositionJ[k]; + A.coeffRef(rowPos, OFFSETVAR(edge.J)) = - 1.0; + + // + gamma (side change) + A.coeffRef(rowPos, GAMMAVAR) = 1; + // >= - gamma + vec_sign[rowPos] = linearProgramming::LP_Constraints::LP_GREATER_OR_EQUAL; + C(rowPos) = 0; + ++rowPos; + } + } +#undef GVAR +#undef OFFSETVAR +#undef GAMMAVAR +} + +struct ConstraintBuilder_GainOffset +{ + ConstraintBuilder_GainOffset( + const std::vector & vec_relativeHistograms, + const std::vector & vec_indexToFix): + _vec_relative(vec_relativeHistograms), + _vec_indexToFix(vec_indexToFix) + { + //Count the number of images + std::set countSet; + for (int i = 0; i < _vec_relative.size(); ++i) + { + countSet.insert(_vec_relative[i].I); + countSet.insert(_vec_relative[i].J); + } + _Nima = countSet.size(); + } + + /// Setup constraints for the translation and structure problem, + /// in the LP_Constraints object. + bool Build(linearProgramming::LP_Constraints_Sparse & constraint) + { + Encode_histo_relation( + _Nima, + _vec_relative, + _vec_indexToFix, + constraint._constraintMat, + constraint._Cst_objective, + constraint._vec_sign, + constraint._vec_cost, + constraint._vec_bounds); + + // it's a minimization problem over the gamma variable + constraint._bminimize = true; + + //-- Setup additional information about the Linear Program constraint + constraint._nbParams = _Nima * 2 + 1; + return true; + } + // Internal data + size_t _Nima; + const std::vector< relativeColorHistogramEdge > & _vec_relative; + const std::vector & _vec_indexToFix; +}; + + +}; // namespace lInfinity +}; // namespace openMVG diff --git a/src/openMVG/color_harmonization/global_quantile_gain_offset_alignment_test.cpp b/src/openMVG/color_harmonization/global_quantile_gain_offset_alignment_test.cpp new file mode 100644 index 0000000000..603d61f027 --- /dev/null +++ b/src/openMVG/color_harmonization/global_quantile_gain_offset_alignment_test.cpp @@ -0,0 +1,208 @@ +// Copyright (c) 2013,2014 Pierre MOULON. + +// 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 http://mozilla.org/MPL/2.0/. + + +#include +#include +#include +#include +#include + +//-- Color harmonization solver +#include "openMVG/color_harmonization/global_quantile_gain_offset_alignment.hpp" + +#include "openMVG/image/image.hpp" +#include "testing/testing.h" + +#include "third_party/histogram/histogram.hpp" +#include "third_party/htmlDoc/htmlDoc.hpp" + + +using namespace openMVG; +using namespace openMVG::linearProgramming; +using namespace openMVG::lInfinity; + +double const pi = 4.0 *std::atan(1.0); + +// simple functor for normal distribution +class normal_distribution +{ +public: + normal_distribution(double m, double s): mu(m), sigma(s) {} + + double operator()() const // returns a single normally distributed number + { + double r1 = (std::rand() + 1.0)/(RAND_MAX + 1.0); // gives equal distribution in (0, 1] + double r2 = (std::rand() + 1.0)/(RAND_MAX + 1.0); + return mu + sigma * std::sqrt(-2*std::log(r1))*std::cos(2*pi*r2); + } +private: + const double mu, sigma; +}; + +TEST(ColorHarmonisation, Simple_offset) { + + Histogram< double > histo( 0, 256, 255); + for (size_t i=0; i < 6000; i++) + { + histo.Add(normal_distribution(127, 10)()); + } + + const size_t OFFET_VALUE = 20; + std::vector vec_reference = histo.GetHist(); + std::vector vec_shifted = vec_reference; + rotate(vec_shifted.begin(), vec_shifted.begin() + OFFET_VALUE, vec_shifted.end()); + + //-- Try to solve the color consistency between the two histograms + //-- We are looking for gain and offet parameter for each image {g;o} + //-- and the upper bound precision found by Linfinity minimization. + std::vector vec_solution(2 * 2 + 1); + + //-- Setup the problem data in the container + std::vector vec_relativeHistograms; + vec_relativeHistograms.push_back(relativeColorHistogramEdge(0,1, vec_reference, vec_shifted)); + //-- First image will be considered as reference and don't move + std::vector vec_indexToFix(1,0); + +#ifdef OPENMVG_HAVE_MOSEK + typedef MOSEK_SolveWrapper SOLVER_LP_T; +#else + typedef OSI_CLP_SolverWrapper SOLVER_LP_T; +#endif + // Red channel + { + SOLVER_LP_T lpSolver(vec_solution.size()); + + ConstraintBuilder_GainOffset cstBuilder(vec_relativeHistograms, vec_indexToFix); + LP_Constraints_Sparse constraint; + cstBuilder.Build(constraint); + lpSolver.setup(constraint); + lpSolver.solve(); + lpSolver.getSolution(vec_solution); + } + + std::cout << "\n\nFound solution:\n"; + std::copy(vec_solution.begin(), vec_solution.end(), + std::ostream_iterator(std::cout, " ")); + + double g0 = vec_solution[0]; + double o0 = vec_solution[1]; + double g1 = vec_solution[2]; + double o1 = vec_solution[3]; + double gamma = vec_solution[4]; + + EXPECT_NEAR(1., g0, 1e-2); + EXPECT_NEAR(0., o0, 1e-2); + EXPECT_NEAR(1., g1, 1e-2); + EXPECT_NEAR(OFFET_VALUE, o1, 1e-2); + EXPECT_NEAR(0., gamma, 1e-2); // Alignment must be perfect +} + +TEST(ColorHarmonisation, Offset_gain) { + + Histogram< double > histo_ref( 0, 256, 255); + Histogram< double > histo_offset_gain( 0, 256, 255); + const double GAIN = 3.0; + const double OFFSET = 160; + //const double GAIN = 2.0; + //const double OFFSET = 50; + for (size_t i=0; i < 10000; i++) + { + double val = normal_distribution(127, 10)(); + histo_ref.Add(val); + histo_offset_gain.Add( (val-127) * GAIN + OFFSET); + } + std::vector vec_reference = histo_ref.GetHist(); + std::vector vec_shifted = histo_offset_gain.GetHist(); + + //-- Try to solve the color consistency between the two histograms + //-- We are looking for gain and offet parameter for each image {g;o} + //-- and the upper bound precision found by Linfinity minimization. + std::vector vec_solution(3 * 2 + 1); + + //-- Setup the problem data in the container + std::vector vec_relativeHistograms; + vec_relativeHistograms.push_back(relativeColorHistogramEdge(0,1, vec_reference, vec_shifted)); + vec_relativeHistograms.push_back(relativeColorHistogramEdge(1,2, vec_shifted, vec_reference)); + vec_relativeHistograms.push_back(relativeColorHistogramEdge(0,2, vec_reference, vec_reference)); + //-- First image will be considered as reference and don't move + std::vector vec_indexToFix(1,0); + +#ifdef OPENMVG_HAVE_MOSEK + typedef MOSEK_SolveWrapper SOLVER_LP_T; +#else + typedef OSI_CLP_SolverWrapper SOLVER_LP_T; +#endif + // Red channel + { + SOLVER_LP_T lpSolver(vec_solution.size()); + + ConstraintBuilder_GainOffset cstBuilder(vec_relativeHistograms, vec_indexToFix); + LP_Constraints_Sparse constraint; + cstBuilder.Build(constraint); + lpSolver.setup(constraint); + lpSolver.solve(); + lpSolver.getSolution(vec_solution); + } + + std::cout << "\n\nFound solution:\n"; + std::copy(vec_solution.begin(), vec_solution.end(), + std::ostream_iterator(std::cout, " ")); + + double g0 = vec_solution[0]; + double o0 = vec_solution[1]; + double g1 = vec_solution[2]; + double o1 = vec_solution[3]; + double g2 = vec_solution[4]; + double o2 = vec_solution[5]; + double gamma = vec_solution[6]; + + // The minimal solution must be {0,1,1/gain, 127-offset/gain,1,0} + // gain and offset 2 must not move since it is equal to reference and link to the reference. + + EXPECT_NEAR(1., g0, 1e-2); + EXPECT_NEAR(0., o0, 1e-2); + EXPECT_NEAR(1./GAIN, g1, 1e-1); + EXPECT_NEAR(127-OFFSET/GAIN, o1, 2); // +/- quantization error (2 gray levels) + EXPECT_NEAR(1., g2, 1e-2); + EXPECT_NEAR(0., o2, 1e-2); + CHECK(gamma < 1.0); // Alignment must be below one gray level + + //-- Visual HTML export + using namespace htmlDocument; + htmlDocument::htmlDocumentStream _htmlDocStream ("Global Multiple-View Color Consistency."); + // Reference histogram + { + htmlDocument::JSXGraphWrapper jsxGraph; + jsxGraph.init("test0",600,300); + jsxGraph.addYChart(histo_ref.GetHist(), "point"); + jsxGraph.UnsuspendUpdate(); + std::vector xBin = histo_ref.GetXbinsValue(); + std::pair< std::pair, std::pair > range = autoJSXGraphViewport(xBin, histo_ref.GetHist()); + jsxGraph.setViewport(range); + jsxGraph.close(); + _htmlDocStream.pushInfo(jsxGraph.toStr()); + } + // Histogram with gain and offset change + { + htmlDocument::JSXGraphWrapper jsxGraph; + jsxGraph.init("test1",600,300); + jsxGraph.addYChart(histo_offset_gain.GetHist(), "point"); + jsxGraph.UnsuspendUpdate(); + std::vector xBin = histo_offset_gain.GetXbinsValue(); + std::pair< std::pair, std::pair > range = autoJSXGraphViewport(xBin, histo_offset_gain.GetHist()); + jsxGraph.setViewport(range); + jsxGraph.close(); + _htmlDocStream.pushInfo(jsxGraph.toStr()); + } + + std::ofstream htmlFileStream( "test.html"); + htmlFileStream << _htmlDocStream.getDoc(); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/src/openMVG/color_harmonization/selection_VLDSegment.hpp b/src/openMVG/color_harmonization/selection_VLDSegment.hpp new file mode 100644 index 0000000000..1d0a0f276a --- /dev/null +++ b/src/openMVG/color_harmonization/selection_VLDSegment.hpp @@ -0,0 +1,123 @@ + +// Copyright (c) 2014 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_COLORHARMONIZATION_VLDSEGMENT_H +#define OPENMVG_COLORHARMONIZATION_VLDSEGMENT_H + +#include "openMVG/color_harmonization/selection_interface.hpp" +#include "openMVG/matching/kvld/kvld.h" +#include "openMVG/matching/kvld/kvld_draw.h" + +namespace openMVG { +namespace color_harmonization { + +class commonDataByPair_VLDSegment : public commonDataByPair +{ + public: + commonDataByPair_VLDSegment( const std::string & sLeftImage, + const std::string & sRightImage, + const std::vector< matching::IndMatch >& vec_PutativeMatches, + const vector< SIOPointFeature >& vec_featsL, + const vector< SIOPointFeature >& vec_featsR): + commonDataByPair( sLeftImage, sRightImage ), + _vec_PutativeMatches( vec_PutativeMatches ), + _vec_featsL( vec_featsL ), _vec_featsR( vec_featsR ) + {} + + virtual ~commonDataByPair_VLDSegment() + {} + + /** + * Put masks to white, images are conserved + * + * \param[out] maskLeft Mask of the left image (initialized to corresponding image size). + * \param[out] maskRight Mask of the right image (initialized to corresponding image size). + * + * \return True. + */ + virtual bool computeMask( + Image< unsigned char > & maskLeft, + Image< unsigned char > & maskRight ) + { + std::vector< matching::IndMatch > vec_KVLDMatches; + Image< RGBColor > image; + Image< unsigned char > imageL, imageR; + + ReadImage( _sLeftImage.c_str(), &image ); + Rgb2Gray( image, &imageL ); + + ReadImage( _sRightImage.c_str(), &image ); + Rgb2Gray( image, &imageR ); + + Image< float > imgA ( imageL.GetMat().cast< float >() ); + Image< float > imgB ( imageR.GetMat().cast< float >() ); + + std::vector< Pair > matchesFiltered, matchesPair; + + for( std::vector< matching::IndMatch >::const_iterator iter_match = _vec_PutativeMatches.begin(); + iter_match != _vec_PutativeMatches.end(); + ++iter_match ) + { + matchesPair.push_back( std::make_pair( iter_match->_i, iter_match->_j ) ); + } + + std::vector< double > vec_score; + + //In order to illustrate the gvld(or vld)-consistant neighbors, the following two parameters has been externalized as inputs of the function KVLD. + openMVG::Mat E = openMVG::Mat::Ones( _vec_PutativeMatches.size(), _vec_PutativeMatches.size() ) * ( -1 ); + // gvld-consistancy matrix, intitialized to -1, >0 consistancy value, -1=unknow, -2=false + std::vector< bool > valide( _vec_PutativeMatches.size(), true );// indices of match in the initial matches, if true at the end of KVLD, a match is kept. + + size_t it_num = 0; + KvldParameters kvldparameters;//initial parameters of KVLD + //kvldparameters.K = 5; + while ( + it_num < 5 && + kvldparameters.inlierRate > + KVLD( + imgA, imgB, + _vec_featsL, _vec_featsR, + matchesPair, matchesFiltered, + vec_score, E, valide, kvldparameters ) ) + { + kvldparameters.inlierRate /= 2; + std::cout<<"low inlier rate, re-select matches with new rate="< _vec_featsL, _vec_featsR; + // Left and Right corresponding index (putatives matches) + std::vector< matching::IndMatch > _vec_PutativeMatches; +}; + +} // namespace color_harmonization +} // namespace openMVG + +#endif // OPENMVG_COLORHARMONIZATION_VLDSEGMENT_H diff --git a/src/openMVG/color_harmonization/selection_fullFrame.hpp b/src/openMVG/color_harmonization/selection_fullFrame.hpp new file mode 100644 index 0000000000..55eed885ce --- /dev/null +++ b/src/openMVG/color_harmonization/selection_fullFrame.hpp @@ -0,0 +1,48 @@ + +// Copyright (c) 2014 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_COLORHARMONIZATION_FULLFRAME_H +#define OPENMVG_COLORHARMONIZATION_FULLFRAME_H + +#include "openMVG/color_harmonization/selection_interface.hpp" + +namespace openMVG { +namespace color_harmonization { + +class commonDataByPair_FullFrame : public commonDataByPair +{ +public: + commonDataByPair_FullFrame( const std::string & sLeftImage, + const std::string & sRightImage ): + commonDataByPair( sLeftImage, sRightImage ) + {} + + virtual ~commonDataByPair_FullFrame() {} + + /** + * Put masks to white, all image is considered as valid pixel selection + * + * \param[out] maskLeft Mask of the left image (initialized to corresponding image size). + * \param[out] maskRight Mask of the right image (initialized to corresponding image size). + * + * \return True. + */ + virtual bool computeMask( Image< unsigned char > & maskLeft, Image< unsigned char > & maskRight ) + { + maskLeft.fill( WHITE ); + maskRight.fill( WHITE ); + return true; + } + +private: + +}; + +} // namespace color_harmonization +} // namespace openMVG + +#endif // OPENMVG_COLORHARMONIZATION_FULLFRAME_H diff --git a/src/openMVG/color_harmonization/selection_interface.hpp b/src/openMVG/color_harmonization/selection_interface.hpp new file mode 100644 index 0000000000..2180bf5da4 --- /dev/null +++ b/src/openMVG/color_harmonization/selection_interface.hpp @@ -0,0 +1,79 @@ + +// Copyright (c) 2014 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_COLORHARMONIZATION_SELECTORINTERFACE_H +#define OPENMVG_COLORHARMONIZATION_SELECTORINTERFACE_H + +#include "openMVG/image/image.hpp" +#include "third_party/histogram/histogram.hpp" +#include + +namespace openMVG { +namespace color_harmonization { + +using namespace std; + +class commonDataByPair +{ +public: + commonDataByPair( const std::string & sLeftImage, + const std::string & sRightImage ): + _sLeftImage( sLeftImage ), _sRightImage( sRightImage ) + {} + + virtual ~commonDataByPair() {} + + /** + * Compute mask forthe two images + * + * \param[out] maskLeft Mask of the left image (initialized to corresponding image size). + * \param[out] maskRight Mask of the right image (initialized to corresponding image size). + * + * \return True if(mask not empty). + */ + virtual bool computeMask( Image< unsigned char > & maskLeft, Image< unsigned char > & maskRight ) = 0; + + /** + * Compute Histogram for the color's masked data + * + * \param[in] mask Binary image to determine acceptable zones + * \param[in] channelIndex selected channel : 0 = red; 1 = green; 2 = blue + * \param[in] image Image with RGB or LAB type + * \param[out] histo Histogram of the left image. + * + */ + template< typename ImageType > + static void computeHisto( + Histogram< double > & histo, + const Image< unsigned char >& mask, + size_t channelIndex, + const Image< ImageType >& image ) + { + for( int j = 0; j < mask.Height(); ++j ) + { + for( int i = 0; i < mask.Width(); ++i ) + { + if( ( int )mask( j, i ) != 0 ) + { + histo.Add( image( j, i )( channelIndex ) ); + } + } + } + } + + const std::string & getLeftImage()const{ return _sLeftImage; } + const std::string & getRightImage()const{ return _sRightImage; } + +protected: + // Left and Right image filenames + std::string _sLeftImage, _sRightImage; +}; + +} // namespace color_harmonization +} // namespace openMVG + +#endif // OPENMVG_COLORHARMONIZATION_SELECTORINTERFACE_H diff --git a/src/openMVG/color_harmonization/selection_matchedPoints.hpp b/src/openMVG/color_harmonization/selection_matchedPoints.hpp new file mode 100644 index 0000000000..47fc242017 --- /dev/null +++ b/src/openMVG/color_harmonization/selection_matchedPoints.hpp @@ -0,0 +1,73 @@ + +// Copyright (c) 2014 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_COLORHARMONIZATION_MATCHESPOINTS_H +#define OPENMVG_COLORHARMONIZATION_MATCHESPOINTS_H + +#include "openMVG/color_harmonization/selection_interface.hpp" +#include "openMVG/matching/indMatch.hpp" +#include "openMVG/features/features.hpp" + +#include + +namespace openMVG { +namespace color_harmonization { + +class commonDataByPair_MatchedPoints : public commonDataByPair +{ +public: + commonDataByPair_MatchedPoints(const std::string & sLeftImage, + const std::string & sRightImage, + const std::vector< matching::IndMatch >& vec_PutativeMatches, + const std::vector< SIOPointFeature >& vec_featsL, + const std::vector< SIOPointFeature >& vec_featsR, + const size_t radius = 1 ): + commonDataByPair( sLeftImage, sRightImage ), + _vec_PutativeMatches( vec_PutativeMatches ), + _vec_featsL( vec_featsL ), _vec_featsR( vec_featsR ), _radius( radius ) + {} + + virtual ~commonDataByPair_MatchedPoints() + {} + + /** + * Fill mask from corresponding points (each point pictured by a disk of radius _radius) + * + * \param[out] maskLeft Mask of the left image (initialized to corresponding image size). + * \param[out] maskRight Mask of the right image (initialized to corresponding image size). + * + * \return True if some pixel have been set to true. + */ + virtual bool computeMask( Image< unsigned char > & maskLeft, Image< unsigned char > & maskRight ) + { + maskLeft.fill(0); + maskRight.fill(0); + for( std::vector< matching::IndMatch >::const_iterator + iter_putativeMatches = _vec_PutativeMatches.begin(); + iter_putativeMatches != _vec_PutativeMatches.end(); + ++iter_putativeMatches ) + { + const SIOPointFeature & L = _vec_featsL[ iter_putativeMatches->_i ]; + const SIOPointFeature & R = _vec_featsR[ iter_putativeMatches->_j ]; + + FilledCircle( L.x(), L.y(), ( int )_radius, ( unsigned char ) 255, &maskLeft ); + FilledCircle( R.x(), R.y(), ( int )_radius, ( unsigned char ) 255, &maskRight ); + } + return _vec_PutativeMatches.size() > 0; + } + +private: + size_t _radius; + std::vector< matching::IndMatch > _vec_PutativeMatches; + std::vector< SIOPointFeature > _vec_featsL; + std::vector< SIOPointFeature > _vec_featsR; +}; + +} // namespace color_harmonization +} // namespace openMVG + +#endif // OPENMVG_COLORHARMONIZATION_MATCHESPOINTS_H diff --git a/src/openMVG/image/image_converter.hpp b/src/openMVG/image/image_converter.hpp index 77d23cabdd..345c5ae082 100644 --- a/src/openMVG/image/image_converter.hpp +++ b/src/openMVG/image/image_converter.hpp @@ -43,6 +43,61 @@ void Rgb2Gray(const ImageIn& imaIn, ImageOut *imaOut) Convert(imaIn(j,i), (*imaOut)(j,i)); } +//-------------------------------------------------------------------------- +// RGB ( unsigned char or int ) to Float +//-------------------------------------------------------------------------- + +template< typename Tin, typename Tout > +inline void convertRGB2Float( + const Tin& valIn, + Tout& valOut, + float factor = 1.0f / 255.f) +{ + for( int channel = 0; channel < 3; ++channel ) + valOut(channel) = (float)((int)(valIn(channel)) * factor); +} + +template< typename ImageIn > +void rgb2Float( const ImageIn& imaIn, + Image< RGBfColor > *imaOut, float factor = 1.0f / 255.f ) +{ + assert( imaIn.Depth() == 3 ); + (*imaOut).resize(imaIn.Width(), imaIn.Height()); + // Convert each int RGB to float RGB values + for( int j = 0; j < imaIn.Height(); ++j ) + for( int i = 0; i < imaIn.Width(); ++i ) + convertRGB2Float( imaIn( j, i ), ( *imaOut )( j, i ), factor ); +} + +//-------------------------------------------------------------------------- +// Float to RGB ( unsigned char or int ) +//-------------------------------------------------------------------------- + + +static void convertFloatToInt( + const RGBfColor& valIn, + RGBColor& valOut, + float factor = 255.f) +{ + for( int channel = 0; channel < 3; ++channel ) + valOut(channel) = (int)(valIn(channel) * factor); +} + + +//template +static void rgbFloat2rgbInt( + const Image< RGBfColor >& imaIn, + Image< RGBColor > *imaOut, + float factor = 255.f ) +{ + assert( imaIn.Depth() == 3 ); + (*imaOut).resize(imaIn.Width(), imaIn.Height()); + // Convert each int RGB to float RGB values + for( int j = 0; j < imaIn.Height(); ++j ) + for( int i = 0; i < imaIn.Width(); ++i ) + convertFloatToInt( imaIn( j, i ), (*imaOut)( j, i ), factor ); +} + } // namespace openMVG #endif // OPENMVG_IMAGE_IMAGE_CONVERTER_HPP diff --git a/src/openMVG/image/image_drawing.hpp b/src/openMVG/image/image_drawing.hpp index f92640d73f..84254754b6 100644 --- a/src/openMVG/image/image_drawing.hpp +++ b/src/openMVG/image/image_drawing.hpp @@ -161,6 +161,42 @@ void DrawCircle(int x, int y, int radius, const Color& col, Image *pim) } } +// Filled circle +// Exterior point computed with bresenham approach +// i.e: DrawCircle +template +void FilledCircle(int x, int y, int radius, const Color& col, Image *pim) +{ + Image &im = *pim; + if ( im.Contains(y + radius, x + radius) + || im.Contains(y + radius, x - radius) + || im.Contains(y - radius, x + radius) + || im.Contains(y - radius, x - radius)) { + int x1 = 0; + int y1 = radius; + int d = radius - 1; + while (y1 >= x1) { + DrawLine(x1 + x, y1 + y, x1 + x, -y1 + y, col, pim); + DrawLine(y1 + x, x1 + y, y1 + x, -x1 + y, col, pim); + DrawLine(-x1 + x, y1 + y, -x1 + x, -y1 + y, col, pim); + DrawLine(-y1 + x, x1 + y, -y1 + x, -x1 + y, col, pim); + if (d >= 2 * x1) { + d = d - 2 * x1 - 1; + x1 += 1; + } else { + if (d <= 2 * (radius - y1)) { + d = d + 2 * y1 - 1; + y1 -= 1; + } else { + d = d + 2 * (y1 - x1 - 1); + y1 -= 1; + x1 += 1; + } + } + } + } +} + // Bresenham algorithm template void DrawLine(int xa, int ya, int xb, int yb, const Color& col, Image *pim) diff --git a/src/openMVG/image/pixel_types.hpp b/src/openMVG/image/pixel_types.hpp index 5fc21b72c9..b1a9d09c05 100644 --- a/src/openMVG/image/pixel_types.hpp +++ b/src/openMVG/image/pixel_types.hpp @@ -66,6 +66,7 @@ class Rgb : public Eigen::Matrix } }; typedef Rgb RGBColor; +typedef Rgb RGBfColor; /// RGBA templated pixel type template diff --git a/src/software/CMakeLists.txt b/src/software/CMakeLists.txt index 0c91179086..7170dec296 100644 --- a/src/software/CMakeLists.txt +++ b/src/software/CMakeLists.txt @@ -1,4 +1,5 @@ ADD_SUBDIRECTORY(globalSfM) ADD_SUBDIRECTORY(SfM) ADD_SUBDIRECTORY(SfMViewer) +ADD_SUBDIRECTORY(colorHarmonize) #ADD_SUBDIRECTORY(opencv) diff --git a/src/software/colorHarmonize/CMakeLists.txt b/src/software/colorHarmonize/CMakeLists.txt new file mode 100644 index 0000000000..441bcb305b --- /dev/null +++ b/src/software/colorHarmonize/CMakeLists.txt @@ -0,0 +1,18 @@ + +SET(LIBS ${EXTRA_LIBS} + openMVG_multiview + openMVG_image + vlsift + lemon + stlplus + ${OPENMVG_LIBRARY_DEPENDENCIES} + openMVG_linearProgramming) + +#Add executable that computes: + +ADD_EXECUTABLE(openMVG_main_ColHarmonize main_ColHarmonize.cpp + colorHarmonizeEngineGlobal.cpp) +TARGET_LINK_LIBRARIES(openMVG_main_ColHarmonize + ${LIBS} + openMVG_kvld) + diff --git a/src/software/colorHarmonize/colorHarmonizeEngineGlobal.cpp b/src/software/colorHarmonize/colorHarmonizeEngineGlobal.cpp new file mode 100644 index 0000000000..2ffb39eaa4 --- /dev/null +++ b/src/software/colorHarmonize/colorHarmonizeEngineGlobal.cpp @@ -0,0 +1,606 @@ + +// Copyright (c) 2013, 2014 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#include "colorHarmonizeEngineGlobal.hpp" +#include "software/SfM/SfMIOHelper.hpp" + +#include "openMVG/image/image.hpp" +//-- Feature matches +#include +#include "openMVG/matching/indMatch_utils.hpp" + +#include "third_party/stlplus3/filesystemSimplified/file_system.hpp" +#include "third_party/vectorGraphics/svgDrawer.hpp" +#include "third_party/stlAddition/stlMap.hpp" +#include "openMVG/matching/indexed_sort.hpp" + +#include "software/globalSfM/indexedImageGraph.hpp" +#include "software/globalSfM/indexedImageGraphExport.hpp" + +//-- Selection Methods +#include "openMVG/color_harmonization/selection_fullFrame.hpp" +#include "openMVG/color_harmonization/selection_matchedPoints.hpp" +#include "openMVG/color_harmonization/selection_VLDSegment.hpp" + +//-- Color harmonization solver +#include "openMVG/color_harmonization/global_quantile_gain_offset_alignment.hpp" + +#include "openMVG/graph/connectedComponent.hpp" +#include "lemon/list_graph.h" + +#include "third_party/progress/progress.hpp" + +#include +#include +#include +#include +#include +#include + + +namespace openMVG{ + +using namespace lemon; +using namespace openMVG::matching; +using namespace openMVG::lInfinity; + +typedef SIOPointFeature FeatureT; +typedef vector< FeatureT > featsT; + +ColorHarmonizationEngineGlobal::ColorHarmonizationEngineGlobal( const string & sImagePath, + const string & sMatchesPath, + const std::string & sMatchesFile, + const string & sOutDirectory, + const int selectionMethod, + const int imgRef): + ReconstructionEngine( sImagePath, sMatchesPath, sOutDirectory ), + _selectionMethod( selectionMethod ), + _imgRef( imgRef ), + _sMatchesFile(sMatchesFile) +{ + if( !stlplus::folder_exists( sOutDirectory ) ) + { + stlplus::folder_create( sOutDirectory ); + } +} + +ColorHarmonizationEngineGlobal::~ColorHarmonizationEngineGlobal() +{ +} + +static void pauseProcess() +{ + unsigned char i; + cout << "\nPause : type key and press enter: "; + std::cin >> i; +} + + +bool ColorHarmonizationEngineGlobal::Process() +{ + const std::string vec_selectionMethod[ 3 ] = { "fullFrame", "matchedPoints", "KVLD" }; + const std::string vec_harmonizeMethod[ 1 ] = { "quantifiedGainCompensation" }; + const int harmonizeMethod = 0; + + //------------------- + // Load data + //------------------- + + if( !ReadInputData() ) + return false; + if( _map_Matches.size() == 0 ) + { + cout << endl << "Matches file is empty" << endl; + return false; + } + + //-- Remove EG with poor support: + + for (map< std::pair, vector >::iterator iter = _map_Matches.begin(); + iter != _map_Matches.end(); + ++iter) + { + if (iter->second.size() < 120) + { + _map_Matches.erase(iter); + iter = _map_Matches.begin(); + } + } + + { + typedef lemon::ListGraph Graph; + imageGraph::indexedImageGraph putativeGraph(_map_Matches, _vec_fileNames); + + // Save the graph before cleaning: + imageGraph::exportToGraphvizData( + stlplus::create_filespec(_sOutDirectory, "input_graph_poor_supportRemoved"), + putativeGraph.g); + } + + //------------------- + // Keep the largest CC in the image graph + //------------------- + if (!CleanGraph()) + { + std::cout << std::endl << "There is no largest CC in the graph" << std::endl; + return false; + } + + //------------------- + //-- Color Harmonization + //------------------- + + //Choose image reference + if( _imgRef == -1 ) + { + do + { + cout << "Choose your reference image:\n"; + for( int i = 0; i < _vec_fileNames.size(); ++i ) + { + cout << "id: " << i << "\t" << _vec_fileNames[ i ] << endl; + } + }while( !( cin >> _imgRef ) || _imgRef < 0 || _imgRef >= _vec_fileNames.size() ); + } + + //Choose selection method + if( _selectionMethod == -1 ) + { + cout << "Choose your selection method:\n" + << "- FullFrame: 0\n" + << "- Matched Points: 1\n" + << "- VLD Segment: 2\n"; + while( ! ( cin >> _selectionMethod ) || _selectionMethod < 0 || _selectionMethod > 2 ) + { + cout << _selectionMethod << " is not accepted.\nTo use: \n- FullFrame enter: 0\n- Matched Points enter: 1\n- VLD Segment enter: 2\n"; + } + } + + //------------------- + // Compute remaining camera node Id + //------------------- + + std::map map_cameraNodeToCameraIndex; // graph node Id to 0->Ncam + std::map map_cameraIndexTocameraNode; // 0->Ncam correspondance to graph node Id + std::set set_indeximage; + for (size_t i = 0; i < _map_Matches.size(); ++i) + { + map< std::pair, vector >::const_iterator iter = _map_Matches.begin(); + std::advance(iter, i); + + const size_t I = iter->first.first; + const size_t J = iter->first.second; + set_indeximage.insert(I); + set_indeximage.insert(J); + } + + for (std::set::const_iterator iterSet = set_indeximage.begin(); + iterSet != set_indeximage.end(); ++iterSet) + { + map_cameraIndexTocameraNode[std::distance(set_indeximage.begin(), iterSet)] = *iterSet; + map_cameraNodeToCameraIndex[*iterSet] = std::distance(set_indeximage.begin(), iterSet); + } + + std::cout << "\n Remaining cameras after CC filter : \n" + << map_cameraIndexTocameraNode.size() << " from a total of " << _vec_fileNames.size() << std::endl; + + size_t bin = 256; + double minvalue = 0.0; + double maxvalue = 255.0; + + // For each edge computes the selection masks and histograms (for the RGB channels) + std::vector map_relativeHistograms[3]; + map_relativeHistograms[0].resize(_map_Matches.size()); + map_relativeHistograms[1].resize(_map_Matches.size()); + map_relativeHistograms[2].resize(_map_Matches.size()); + + for (size_t i = 0; i < _map_Matches.size(); ++i) + { + map< std::pair, vector >::const_iterator iter = _map_Matches.begin(); + std::advance(iter, i); + + const size_t I = iter->first.first; + const size_t J = iter->first.second; + + const std::vector & vec_matchesInd = iter->second; + + //-- Edges names: + std::pair< std::string, std::string > p_imaNames; + p_imaNames = make_pair( stlplus::create_filespec( _sImagePath, _vec_fileNames[ I ] ), + stlplus::create_filespec( _sImagePath, _vec_fileNames[ J ] ) ); + std::cout << "Current edge : " + << stlplus::filename_part(p_imaNames.first) << "\t" + << stlplus::filename_part(p_imaNames.second) << std::endl; + + //-- Compute the masks from the data selection: + Image< unsigned char > maskI ( _vec_imageSize[ I ].first, _vec_imageSize[ I ].second ); + Image< unsigned char > maskJ ( _vec_imageSize[ J ].first, _vec_imageSize[ J ].second ); + + switch(_selectionMethod) + { + enum EHistogramSelectionMethod + { + eHistogramHarmonizeFullFrame = 0, + eHistogramHarmonizeMatchedPoints = 1, + eHistogramHarmonizeVLDSegment = 2, + }; + + case eHistogramHarmonizeFullFrame: + { + color_harmonization::commonDataByPair_FullFrame dataSelector( + p_imaNames.first, + p_imaNames.second); + dataSelector.computeMask( maskI, maskJ ); + } + break; + case eHistogramHarmonizeMatchedPoints: + { + int circleSize = 10; + color_harmonization::commonDataByPair_MatchedPoints dataSelector( + p_imaNames.first, + p_imaNames.second, + vec_matchesInd, + _map_feats[ I ], + _map_feats[ J ], + circleSize); + dataSelector.computeMask( maskI, maskJ ); + } + break; + case eHistogramHarmonizeVLDSegment: + { + color_harmonization::commonDataByPair_VLDSegment dataSelector( + p_imaNames.first, + p_imaNames.second, + vec_matchesInd, + _map_feats[ I ], + _map_feats[ J ]); + + dataSelector.computeMask( maskI, maskJ ); + } + break; + default: + std::cout << "Selection method unsupported" << std::endl; + return false; + } + + //-- Export the masks + bool bExportMask = false; + if (bExportMask) + { + string sEdge = _vec_fileNames[ I ] + "_" + _vec_fileNames[ J ]; + sEdge = stlplus::create_filespec( _sOutDirectory, sEdge ); + if( !stlplus::folder_exists( sEdge ) ) + stlplus::folder_create( sEdge ); + + string out_filename_I = "00_mask_I.png"; + out_filename_I = stlplus::create_filespec( sEdge, out_filename_I ); + + string out_filename_J = "00_mask_J.png"; + out_filename_J = stlplus::create_filespec( sEdge, out_filename_J ); + + WriteImage( out_filename_I.c_str(), maskI ); + WriteImage( out_filename_J.c_str(), maskJ ); + } + + //-- Compute the histograms + Image< RGBColor > imageI, imageJ; + ReadImage( p_imaNames.first.c_str(), &imageI ); + ReadImage( p_imaNames.second.c_str(), &imageJ ); + + Histogram< double > histoI( minvalue, maxvalue, bin); + Histogram< double > histoJ( minvalue, maxvalue, bin); + + int channelIndex = 0; // RED channel + color_harmonization::commonDataByPair::computeHisto( histoI, maskI, channelIndex, imageI ); + color_harmonization::commonDataByPair::computeHisto( histoJ, maskJ, channelIndex, imageJ ); + relativeColorHistogramEdge & edgeR = map_relativeHistograms[channelIndex][i]; + edgeR = relativeColorHistogramEdge(map_cameraNodeToCameraIndex[I], map_cameraNodeToCameraIndex[J], + histoI.GetHist(), histoJ.GetHist()); + + histoI = histoJ = Histogram< double >( minvalue, maxvalue, bin); + channelIndex = 1; // GREEN channel + color_harmonization::commonDataByPair::computeHisto( histoI, maskI, channelIndex, imageI ); + color_harmonization::commonDataByPair::computeHisto( histoJ, maskJ, channelIndex, imageJ ); + relativeColorHistogramEdge & edgeG = map_relativeHistograms[channelIndex][i]; + edgeG = relativeColorHistogramEdge(map_cameraNodeToCameraIndex[I], map_cameraNodeToCameraIndex[J], + histoI.GetHist(), histoJ.GetHist()); + + histoI = histoJ = Histogram< double >( minvalue, maxvalue, bin); + channelIndex = 2; // BLUE channel + color_harmonization::commonDataByPair::computeHisto( histoI, maskI, channelIndex, imageI ); + color_harmonization::commonDataByPair::computeHisto( histoJ, maskJ, channelIndex, imageJ ); + relativeColorHistogramEdge & edgeB = map_relativeHistograms[channelIndex][i]; + edgeB = relativeColorHistogramEdge(map_cameraNodeToCameraIndex[I], map_cameraNodeToCameraIndex[J], + histoI.GetHist(), histoJ.GetHist()); + } + + std::cout << "\n -- \n SOLVE for color consistency with linear programming\n --" << std::endl; + //-- Solve for the gains and offsets: + std::vector vec_indexToFix; + vec_indexToFix.push_back(map_cameraNodeToCameraIndex[_imgRef]); + + using namespace openMVG::linearProgramming; + + std::vector vec_solution_r(_vec_fileNames.size() * 2 + 1); + std::vector vec_solution_g(_vec_fileNames.size() * 2 + 1); + std::vector vec_solution_b(_vec_fileNames.size() * 2 + 1); + + clock_t timeStart = clock(); + + #ifdef OPENMVG_HAVE_MOSEK + typedef MOSEK_SolveWrapper SOLVER_LP_T; + #else + typedef OSI_CLP_SolverWrapper SOLVER_LP_T; + #endif + // Red channel + { + SOLVER_LP_T lpSolver(vec_solution_r.size()); + + ConstraintBuilder_GainOffset cstBuilder(map_relativeHistograms[0], vec_indexToFix); + LP_Constraints_Sparse constraint; + cstBuilder.Build(constraint); + lpSolver.setup(constraint); + lpSolver.solve(); + lpSolver.getSolution(vec_solution_r); + } + // Green channel + { + SOLVER_LP_T lpSolver(vec_solution_g.size()); + + ConstraintBuilder_GainOffset cstBuilder(map_relativeHistograms[1], vec_indexToFix); + LP_Constraints_Sparse constraint; + cstBuilder.Build(constraint); + lpSolver.setup(constraint); + lpSolver.solve(); + lpSolver.getSolution(vec_solution_g); + } + // Blue channel + { + SOLVER_LP_T lpSolver(vec_solution_b.size()); + + ConstraintBuilder_GainOffset cstBuilder(map_relativeHistograms[2], vec_indexToFix); + LP_Constraints_Sparse constraint; + cstBuilder.Build(constraint); + lpSolver.setup(constraint); + lpSolver.solve(); + lpSolver.getSolution(vec_solution_b); + } + + clock_t timeEnd = clock(); + std::cout << std::endl + << " ColorHarmonization solving on a graph with: " << _map_Matches.size() << " edges took : " + << (timeEnd - timeStart) / (double)CLOCKS_PER_SEC << " seconds." << std::endl + << "LInfinity fitting error: \n" + << "- for the red channel is: " << vec_solution_r.back() << " gray level(s)" <(std::cout, " ")); + + std::cout << "\n\nFound solution_g:\n"; + std::copy(vec_solution_g.begin(), vec_solution_g.end(), std::ostream_iterator(std::cout, " ")); + + std::cout << "\n\nFound solution_b:\n"; + std::copy(vec_solution_b.begin(), vec_solution_b.end(), std::ostream_iterator(std::cout, " ")); + std::cout << std::endl; + + std::cout << "\n\nThere is :\n" << set_indeximage.size() << " images to transform." << std::endl; + + //-> convert solution to gain offset and creation of the LUT per image + C_Progress_display my_progress_bar( set_indeximage.size() ); + for (std::set::const_iterator iterSet = set_indeximage.begin(); + iterSet != set_indeximage.end(); ++iterSet, ++my_progress_bar) + { + size_t imaNum = *iterSet; + typedef Eigen::Matrix Vec256; + std::vector< Vec256 > vec_map_lut(3); + + size_t nodeIndex = std::distance(set_indeximage.begin(), iterSet); + + double g_r = vec_solution_r[nodeIndex*2]; + double offset_r = vec_solution_r[nodeIndex*2+1]; + double g_g = vec_solution_g[nodeIndex*2]; + double offset_g = vec_solution_g[nodeIndex*2+1]; + double g_b = vec_solution_b[nodeIndex*2]; + double offset_b = vec_solution_b[nodeIndex*2+1]; + + for( size_t k = 0; k < 256; ++k) + { + vec_map_lut[0][k] = clamp( k * g_r + offset_r, 0., 255. ); + vec_map_lut[1][k] = clamp( k * g_g + offset_g, 0., 255. ); + vec_map_lut[2][k] = clamp( k * g_b + offset_b, 0., 255. ); + } + + Image< RGBColor > image_c; + ReadImage( stlplus::create_filespec( _sImagePath, _vec_fileNames[ imaNum ] ).c_str(), &image_c ); + +#ifdef USE_OPENMP +#pragma omp parallel for +#endif + for( int j = 0; j < image_c.Height(); ++j ) + { + for( int i = 0; i < image_c.Width(); ++i ) + { + image_c(j, i)[0] = clamp(vec_map_lut[0][image_c(j, i)[0]], 0., 255.); + image_c(j, i)[1] = clamp(vec_map_lut[1][image_c(j, i)[1]], 0., 255.); + image_c(j, i)[2] = clamp(vec_map_lut[2][image_c(j, i)[2]], 0., 255.); + } + } + + std::string out_folder = stlplus::create_filespec( _sOutDirectory, + vec_selectionMethod[ _selectionMethod ] + "_" + vec_harmonizeMethod[ harmonizeMethod ]); + if( !stlplus::folder_exists( out_folder ) ) + stlplus::folder_create( out_folder ); + std::string out_filename = stlplus::create_filespec( out_folder, _vec_fileNames[ imaNum ] ); + + WriteImage( out_filename.c_str(), image_c ); + } + return true; +} + +bool ColorHarmonizationEngineGlobal::ReadInputData() +{ + if( !stlplus::is_folder( _sImagePath ) || + !stlplus::is_folder( _sMatchesPath) || + !stlplus::is_folder( _sOutDirectory) ) + { + cerr << endl + << "One of the required directory is not a valid directory" << endl; + return false; + } + + string sListsFile = stlplus::create_filespec( _sMatchesPath,"lists","txt" ); + if( !stlplus::is_file( sListsFile ) || + !stlplus::is_file( _sMatchesFile ) ) + { + cerr << endl + << "One of the input required file is not a present (lists.txt," + << stlplus::basename_part(_sMatchesFile) << ")" << endl; + return false; + } + + // a. Read images names + { + std::vector vec_camImageNames; + std::vector vec_intrinsicGroups; + if (!openMVG::SfMIO::loadImageList( vec_camImageNames, + vec_intrinsicGroups, + sListsFile) ) + { + cerr << "\nEmpty image list." << endl; + return false; + } + + for ( std::vector::const_iterator iter_imageName = vec_camImageNames.begin(); + iter_imageName != vec_camImageNames.end(); + iter_imageName++ ) + { + _vec_fileNames.push_back( iter_imageName->m_sImageName ); + + _vec_imageSize.push_back( make_pair( vec_intrinsicGroups[iter_imageName->m_intrinsicId].m_w, + vec_intrinsicGroups[iter_imageName->m_intrinsicId].m_h ) ); + } + } + + // b. Read matches + if( !matching::PairedIndMatchImport( _sMatchesFile, _map_Matches ) ) + { + cerr<< "Unable to read the geometric matrix matches" << endl; + return false; + } + + // Read features: + for( size_t i = 0; i < _vec_fileNames.size(); ++i ) + { + const size_t camIndex = i; + if( !loadFeatsFromFile( + stlplus::create_filespec( _sMatchesPath, + stlplus::basename_part( _vec_fileNames[ camIndex ] ), + ".feat" ), + _map_feats[ camIndex ] ) ) + { + cerr << "Bad reading of feature files" << endl; + return false; + } + } + + using namespace lemon; + + typedef lemon::ListGraph Graph; + imageGraph::indexedImageGraph putativeGraph( _map_Matches, _vec_fileNames ); + + // Save the graph before cleaning: + imageGraph::exportToGraphvizData( + stlplus::create_filespec( _sOutDirectory, "initialGraph" ), + putativeGraph.g ); + + return true; +} + +bool ColorHarmonizationEngineGlobal::CleanGraph() +{ + // Create a graph from pairwise correspondences: + // - keep the largest connected component. + + typedef lemon::ListGraph Graph; + imageGraph::indexedImageGraph putativeGraph(_map_Matches, _vec_fileNames); + + // Save the graph before cleaning: + imageGraph::exportToGraphvizData( + stlplus::create_filespec(_sOutDirectory, "initialGraph"), + putativeGraph.g); + + int connectedComponentCount = lemon::countConnectedComponents(putativeGraph.g); + std::cout << "\n" + << "ColorHarmonizationEngineGlobal::CleanGraph() :: => connected Component cardinal: " + << connectedComponentCount << std::endl; + using namespace openMVG::imageGraph; + if (connectedComponentCount > 1) // If more than one CC, keep the largest + { + // Search the largest CC index + const std::map > map_subgraphs = + openMVG::graphUtils::exportGraphToMapSubgraphs(putativeGraph.g); + size_t count = std::numeric_limits::min(); + std::map >::const_iterator iterLargestCC = map_subgraphs.end(); + for(std::map >::const_iterator iter = map_subgraphs.begin(); + iter != map_subgraphs.end(); ++iter) + { + if (iter->second.size() > count) { + count = iter->second.size(); + iterLargestCC = iter; + } + std::cout << "Connected component of size : " << iter->second.size() << std::endl; + } + + //-- Remove all nodes that are not listed in the largest CC + for(std::map >::const_iterator iter = map_subgraphs.begin(); + iter != map_subgraphs.end(); ++iter) + { + if (iter == iterLargestCC) // Skip this CC since it's the one we want to keep + continue; + + const std::set & ccSet = iter->second; + for (std::set::const_iterator iter2 = ccSet.begin(); + iter2 != ccSet.end(); ++iter2) + { + // Remove all outgoing edges + for (Graph::OutArcIt e(putativeGraph.g, *iter2); e!=INVALID; ++e) + { + putativeGraph.g.erase(e); + size_t Idu = (*putativeGraph.map_nodeMapIndex)[putativeGraph.g.target(e)]; + size_t Idv = (*putativeGraph.map_nodeMapIndex)[putativeGraph.g.source(e)]; + openMVG::tracks::STLPairWiseMatches::iterator iterM = _map_Matches.find(std::make_pair(Idu,Idv)); + if( iterM != _map_Matches.end()) + { + _map_Matches.erase(iterM); + } + else // Try to find the opposite directed edge + { + iterM = _map_Matches.find(std::make_pair(Idv,Idu)); + if( iterM != _map_Matches.end()) + _map_Matches.erase(iterM); + } + } + } + } + } + + // Save the graph after cleaning: + imageGraph::exportToGraphvizData( + stlplus::create_filespec(_sOutDirectory, "cleanedGraph"), + putativeGraph.g); + + std::cout << "\n" + << "Cardinal of nodes: " << lemon::countNodes(putativeGraph.g) << "\n" + << "Cardinal of edges: " << lemon::countEdges(putativeGraph.g) << std::endl + << std::endl; + + return true; +} + +} // namespace openMVG diff --git a/src/software/colorHarmonize/colorHarmonizeEngineGlobal.hpp b/src/software/colorHarmonize/colorHarmonizeEngineGlobal.hpp new file mode 100644 index 0000000000..24f1e59be6 --- /dev/null +++ b/src/software/colorHarmonize/colorHarmonizeEngineGlobal.hpp @@ -0,0 +1,68 @@ + +// Copyright (c) 2013, 2014 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#ifndef OPENMVG_COLOR_HARMONIZATION_ENGINE_GLOBAL_H +#define OPENMVG_COLOR_HARMONIZATION_ENGINE_GLOBAL_H + +#include +#include +#include +#include + +#include + +namespace openMVG{ + +class ColorHarmonizationEngineGlobal : public ReconstructionEngine +{ +public: + ColorHarmonizationEngineGlobal( const std::string & sImagePath, + const std::string & sMatchesPath, + const std::string & sMatchesFile, + const std::string & sOutDirectory, + const int selectionMethod = -1, + const int imgRef = -1); + + ~ColorHarmonizationEngineGlobal(); + + virtual bool Process(); + +private: + + bool CleanGraph(); + + /// Read input data (point correspondences) + bool ReadInputData(); + +public: + + const std::vector< std::string > & getFilenamesVector() const { return _vec_fileNames; } + + const std::vector< std::pair< size_t, size_t > > & getImagesSize() const { return _vec_imageSize; } + +private: + + int _selectionMethod; + int _imgRef; + std::string _sMatchesFile; + + // ----- + // Input data + // ---- + + std::vector< std::string > _vec_fileNames; // considered images + std::map< size_t, std::vector< SIOPointFeature > > _map_feats; // feature per images + + std::vector< std::pair< size_t, size_t > > _vec_imageSize; // Size of each image + + openMVG::tracks::STLPairWiseMatches _map_Matches; // pairwise geometric matches +}; + + +} // namespace openMVG + +#endif // OPENMVG_COLOR_HARMONIZATION_ENGINE_GLOBAL_H diff --git a/src/software/colorHarmonize/main_ColHarmonize.cpp b/src/software/colorHarmonize/main_ColHarmonize.cpp new file mode 100644 index 0000000000..05f6583e4f --- /dev/null +++ b/src/software/colorHarmonize/main_ColHarmonize.cpp @@ -0,0 +1,98 @@ + +// Copyright (c) 2013, 2014 openMVG authors. + +// 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 http://mozilla.org/MPL/2.0/. + +#include +#include + +#include "software/colorHarmonize/colorHarmonizeEngineGlobal.hpp" + +#include "third_party/cmdLine/cmdLine.h" +#include "third_party/stlplus3/filesystemSimplified/file_system.hpp" + +using namespace openMVG; + +int main( int argc, char **argv ) +{ + using namespace std; + std::cout << "Global Color Harmonization" << std::endl + << std::endl; + + CmdLine cmd; + + std::string sImaDirectory; + std::string sMatchesDir, sMatchesFile; + std::string sOutDir = ""; + int selectionMethod = -1; + int imgRef = -1; + + cmd.add( make_option( 'i', sImaDirectory, "imagesDirectory" ) ); + cmd.add( make_option( 'm', sMatchesFile, "matchesFile" ) ); + cmd.add( make_option( 'o', sOutDir, "outdir" ) ); + cmd.add( make_option( 's', selectionMethod, "selectionMethod" ) ); + cmd.add( make_option( 'r', imgRef, "referenceImage" ) ); + + try + { + if( argc == 1 ) throw std::string( "Invalid command line parameter." ); + cmd.process( argc, argv ); + } + catch( const std::string& s ) + { + std::cerr << "Usage: " << argv[ 0 ] << ' ' + << "[-i|--imagesDirectory path] " + << "[-m|--sMatchesFile path] " + << "[-o|--outdir path] " + << "[-s|--selectionMethod int] " + << "[-r|--referenceImage int]" + << std::endl; + + std::cerr << s << std::endl; + return EXIT_FAILURE; + } + + if ( sImaDirectory.empty() ) + { + std::cerr << "\nIt is an invalid output directory" << std::endl; + return EXIT_FAILURE; + } + + if ( !stlplus::folder_exists( sOutDir ) ) + stlplus::folder_create( sOutDir ); + + //--------------------------------------- + // Harmonization process + //--------------------------------------- + + clock_t timeStart = clock(); + + sMatchesDir = stlplus::folder_part(sMatchesFile); + std::auto_ptr m_colorHarmonizeEngine; + { + m_colorHarmonizeEngine = std::auto_ptr( + new ColorHarmonizationEngineGlobal(sImaDirectory, + sMatchesDir, + sMatchesFile, + sOutDir, + selectionMethod, + imgRef)); + } + + if ( m_colorHarmonizeEngine->Process() ) + { + clock_t timeEnd = clock(); + std::cout << std::endl + << " ColorHarmonization took : " + << (timeEnd - timeStart) / CLOCKS_PER_SEC << " seconds." << std::endl; + + return EXIT_SUCCESS; + } + else + { + std::cerr << "\n Something goes wrong in the process" << std::endl; + } + return EXIT_FAILURE; +} From b00c7e2d8b3ef8db19b55f2c4502e95402fda809 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Fri, 31 Jan 2014 16:42:13 +0100 Subject: [PATCH 42/68] Fix some english typo. --- src/openMVG/matching/kvld/kvld.cpp | 13 ++++++------- src/openMVG/matching/kvld/kvld.h | 26 +++++++++++++------------- 2 files changed, 19 insertions(+), 20 deletions(-) diff --git a/src/openMVG/matching/kvld/kvld.cpp b/src/openMVG/matching/kvld/kvld.cpp index a813b0b6d1..ebf72d7475 100644 --- a/src/openMVG/matching/kvld/kvld.cpp +++ b/src/openMVG/matching/kvld/kvld.cpp @@ -250,7 +250,6 @@ float KVLD( const Image< float >& I1, //============main iteration formatch verification==========// // cout<<"main iteration"; bool change = true; - bool initial = true; while( change ) { @@ -436,7 +435,7 @@ float KVLD( const Image< float >& I1, } -void writeResult( const string output, +void writeResult( const std::string & output, const vector< openMVG::SIOPointFeature >& vec_F1, const vector< openMVG::SIOPointFeature >& vec_F2, const vector< Pair >& vec_matches, @@ -451,7 +450,7 @@ void writeResult( const string output, feature1 << vec_F1.size() << endl; for( vector< openMVG::SIOPointFeature >::const_iterator it = vec_F1.begin(); it != vec_F1.end(); - it++ ) + ++it ) { writeDetector( feature1, ( *it ) ); } @@ -463,7 +462,7 @@ void writeResult( const string output, feature2 << vec_F2.size() << endl; for( vector< openMVG::SIOPointFeature >::const_iterator it = vec_F2.begin(); it != vec_F2.end(); - it++ ) + ++it ) { writeDetector( feature2, ( *it ) ); } @@ -474,7 +473,7 @@ void writeResult( const string output, if( !initialmatches.is_open() ) cout << "error while writing initial_matches.txt" << endl; initialmatches << vec_matches.size() << endl; - for( vector< Pair >::const_iterator it = vec_matches.begin(); it != vec_matches.end(); it++ ) + for( vector< Pair >::const_iterator it = vec_matches.begin(); it != vec_matches.end(); ++it ) { initialmatches << it->first << " " << it->second << endl; } @@ -488,7 +487,7 @@ void writeResult( const string output, filteredmatches << vec_matchesFiltered.size() << endl; for( vector< Pair >::const_iterator it = vec_matchesFiltered.begin(); it != vec_matchesFiltered.end(); - it++ ) + ++it ) { filteredmatches << it->first << " " << it->second << endl; @@ -502,7 +501,7 @@ void writeResult( const string output, for( vector< double >::const_iterator it = vec_score.begin(); it != vec_score.end(); - it++ ) + ++it ) { filteredmatches << *it << endl; } diff --git a/src/openMVG/matching/kvld/kvld.h b/src/openMVG/matching/kvld/kvld.h index 223708cb9c..5a538475c3 100644 --- a/src/openMVG/matching/kvld/kvld.h +++ b/src/openMVG/matching/kvld/kvld.h @@ -31,13 +31,13 @@ the terms of the BSD license (see the COPYING file). const float maxContrast = 300.0f; //===inner parameters of VLD, usually not to change - const int dimension = 10; //number of simplified SIFT-like chaines used in a single vld - const int subdirection = 8; // number of bins in a SIFT-like chaine's histogramme - const int binNum = 24;//number of bins for SIFT-like chaines' main direction. Must be a pair number + const int dimension = 10; //number of simplified SIFT-like chain used in a single vld + const int subdirection = 8; // number of bins in a SIFT-like chain histogram + const int binNum = 24;//number of bins for SIFT-like chain main direction. Must be a pair number //===== initialize parameters for a KVLD process ====// // inlierRate: the minimum rate down to which the KVLD should be reprocessed with a lower number of inlierRate, initially set to 0.04 -// K: the minumum number of gvld-consistent (or vld-consistent) neighbors to select a match as correct one +// K: the minimum number of gvld-consistent (or vld-consistent) neighbors to select a match as correct one // geometry: if true, KVLD will also take geometric verification into account. c.f. paper // if false, KVLD execute a pure photometric verification struct KvldParameters @@ -48,12 +48,12 @@ struct KvldParameters KvldParameters(): inlierRate( 0.04 ), K( 3 ), geometry( true ){}; }; -//====== Pyramide of scale images ======// -// elements as angles[0] and magnitudes[0] presents orientations and gradient normes of the original scale (biggest), and the following elements +//====== Pyramid of scale images ======// +// elements as angles[0] and magnitudes[0] presents orientations and gradient norms of the original scale (biggest), and the following elements // are scaled down by a step of sqrt(2.0) in size // -// angles: strore orientations of pixels of each scale image into a vector of images, which varies from 0 to 2*PI for each pixel -// magnitudes: strore gradient normes of pixels of each scale image into a vector of images +// angles: store orientations of pixels of each scale image into a vector of images, which varies from 0 to 2*PI for each pixel +// magnitudes: store gradient norms of pixels of each scale image into a vector of images struct ImageScale { std::vector< Image< float > > angles; @@ -62,7 +62,7 @@ struct ImageScale double radius_size; double step; - ImageScale( const Image< float >& I,double r = 5.0 ); + ImageScale( const Image< float >& I,double r = 5.0 ); int getIndex( const double r )const; private: @@ -156,10 +156,10 @@ class VLD //matches: initial possible matches containing pairs of INDEX of matches features from F1 and F2. An element of matches as (1,3) present the 1st feature in F1 // and the 3rd feature in F2 // -//matchesFiltered: the output list of matches filled by KVLD processe.(it will be cleared to empty at the beginning of KVLD). +//matchesFiltered: the output list of matches filled by KVLD process.(it will be cleared to empty at the beginning of KVLD). // //score: list of score for output matches. If geometric verification is set to true, each element presents the geometric consistancy score of the corresponding match with its neighbor matches -// otherwise, it presents the average photometric consistancy score with its neighbor matches. +// otherwise, it presents the average photometric consistency score with its neighbor matches. // //E: gvld(or vld)-consistency matrix, for illustration reason, it has been externalized as an input of KVLD. it should be initialized to be a matches.size*matche.sizes table with all equal to -1 // e.g. libNumerics::matrix E = libNumerics::matrix::ones(matchesPair.size(),matchesPair.size())*(-1); @@ -167,7 +167,7 @@ class VLD //valide: indices of whether the i th match in the initial match list is selected, for illustration reason, it has been externalized as an input of KVLD. it should be initialized to be a // matches.size vector with all equal to true. e.g. std::vector valide(size, true); // -//kvldParameters: container of miminum inlier rate, the value of K (=3 initially) and geometric verification flag (true initially) +//kvldParameters: container of minimum inlier rate, the value of K (=3 initially) and geometric verification flag (true initially) float KVLD( const Image< float >& I1, const Image< float >& I2, @@ -182,7 +182,7 @@ float KVLD( const Image< float >& I1, //====================KVLD interface======================// -void writeResult( const std::string output, +void writeResult( const std::string & output, const std::vector< openMVG::SIOPointFeature >& F1, const std::vector< openMVG::SIOPointFeature >& F2, const std::vector< Pair >& matches, From 2b58c2c3dd3a6b43016020df9e1d36284b53c90d Mon Sep 17 00:00:00 2001 From: pmoulon Date: Fri, 31 Jan 2014 16:46:25 +0100 Subject: [PATCH 43/68] Minor change suggested by Cppcheck --- src/openMVG/exif_IO/exif_IO_openExif.hpp | 12 ++++++------ src/openMVG/image/image_io.cpp | 2 +- .../global_translations_fromTriplets_test.cpp | 2 +- .../linearProgramming/linearProgrammingMOSEK.cpp | 4 ---- src/openMVG/matching/kvld/algorithm.h | 2 +- src/openMVG/multiview/projection.cpp | 6 +----- .../robust_estimator_ACRansac_test.cpp | 1 - src/openMVG/split/split.hpp | 2 +- 8 files changed, 11 insertions(+), 20 deletions(-) diff --git a/src/openMVG/exif_IO/exif_IO_openExif.hpp b/src/openMVG/exif_IO/exif_IO_openExif.hpp index dae480f0d1..ea5d9692db 100644 --- a/src/openMVG/exif_IO/exif_IO_openExif.hpp +++ b/src/openMVG/exif_IO/exif_IO_openExif.hpp @@ -122,7 +122,7 @@ class Exif_IO_OpenExif: public Exif_IO std::cout << "\nApp1 - \"Exif\" entries:" << std::endl; for (ExifPathsTags::const_iterator crntPathsTags = app1PathsTags_.begin(); crntPathsTags != app1PathsTags_.end(); - crntPathsTags++ ) + ++crntPathsTags ) { ExifIFDPath::const_iterator crntPath = (*crntPathsTags).first.begin(); ExifIFDPath::const_iterator endPath = (*crntPathsTags).first.end(); @@ -130,7 +130,7 @@ class Exif_IO_OpenExif: public Exif_IO { std::cout << "IFD: " << (*crntPath).first << " Idx: " << (*crntPath).second << std::endl; - crntPath++; + ++crntPath; } ExifTags::const_iterator crnt = (*crntPathsTags).second.begin(); @@ -142,7 +142,7 @@ class Exif_IO_OpenExif: public Exif_IO ExifTagEntry* tag = *(crnt); tag->print(); std::cout << std::endl; - crnt++; + ++crnt; } } @@ -151,7 +151,7 @@ class Exif_IO_OpenExif: public Exif_IO std::cout << "\nApp3 - \"Meta\" entries:" << std::endl; for (ExifPathsTags::const_iterator crntPathsTags = app3PathsTags_.begin(); crntPathsTags != app3PathsTags_.end(); - crntPathsTags++ ) + ++crntPathsTags ) { ExifIFDPath::const_iterator crntPath = (*crntPathsTags).first.begin(); ExifIFDPath::const_iterator endPath = (*crntPathsTags).first.end(); @@ -159,7 +159,7 @@ class Exif_IO_OpenExif: public Exif_IO { std::cout << "IFD: " << (*crntPath).first << " Idx: " << (*crntPath).second << std::endl; - crntPath++; + ++crntPath; } ExifTags::const_iterator crnt = (*crntPathsTags).second.begin(); @@ -170,7 +170,7 @@ class Exif_IO_OpenExif: public Exif_IO ExifTagEntry* tag = *(crnt); tag->print(); std::cout << std::endl; - crnt++; + ++crnt; } } diff --git a/src/openMVG/image/image_io.cpp b/src/openMVG/image/image_io.cpp index 56529c1342..aa1afd15fb 100644 --- a/src/openMVG/image/image_io.cpp +++ b/src/openMVG/image/image_io.cpp @@ -109,7 +109,7 @@ struct my_error_mgr { METHODDEF(void) jpeg_error (j_common_ptr cinfo) { - my_error_mgr *myerr = (my_error_mgr*) cinfo->err; + my_error_mgr *myerr = (my_error_mgr*) (cinfo->err); (*cinfo->err->output_message) (cinfo); longjmp(myerr->setjmp_buffer, 1); } diff --git a/src/openMVG/linearProgramming/lInfinityCV/global_translations_fromTriplets_test.cpp b/src/openMVG/linearProgramming/lInfinityCV/global_translations_fromTriplets_test.cpp index 7b47667716..c2503df99a 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/global_translations_fromTriplets_test.cpp +++ b/src/openMVG/linearProgramming/lInfinityCV/global_translations_fromTriplets_test.cpp @@ -55,9 +55,9 @@ void visibleCamPosToSVGSurface( out[i](2) = ((out[i](2) * yfactor) + -yfactor * mean(2)) * 30 + 100; } - double size = 200; if (!fileName.empty()) { + const double size = 200; svgDrawer svgSurface_GT(size,size); for(size_t i = 0; i < vec_Ci.size(); ++i) { diff --git a/src/openMVG/linearProgramming/linearProgrammingMOSEK.cpp b/src/openMVG/linearProgramming/linearProgrammingMOSEK.cpp index 7a2225fe6a..7f64825b7e 100644 --- a/src/openMVG/linearProgramming/linearProgrammingMOSEK.cpp +++ b/src/openMVG/linearProgramming/linearProgrammingMOSEK.cpp @@ -81,7 +81,6 @@ bool MOSEK_SolveWrapper::setup(const LP_Constraints & cstraints) //cstraints <-> { assert(_nbParams == cstraints._nbParams); - bool bOk = true; MSK_deletetask(&task); int NUMVAR = cstraints._constraintMat.cols(); @@ -204,7 +203,6 @@ bool MOSEK_SolveWrapper::setup(const LP_Constraints_Sparse & cstraints) //cstrai { assert(_nbParams == cstraints._nbParams); - bool bOk = true; MSK_deletetask(&task); int NUMVAR = this->_nbParams; @@ -359,7 +357,6 @@ bool MOSEK_SolveWrapper::solve() if ( r== MSK_RES_OK ) { MSKsolstae solsta; - int j; MSK_getsolutionstatus (task, MSK_SOL_BAS, NULL, @@ -377,7 +374,6 @@ bool MOSEK_SolveWrapper::solve() case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER: //printf("Primal or dual infeasibility certificate found.\n"); break; - case MSK_SOL_STA_UNKNOWN: //printf("The status of the solution could not be determined.\n"); break; diff --git a/src/openMVG/matching/kvld/algorithm.h b/src/openMVG/matching/kvld/algorithm.h index b5b5fa57be..d711316cf2 100644 --- a/src/openMVG/matching/kvld/algorithm.h +++ b/src/openMVG/matching/kvld/algorithm.h @@ -40,7 +40,7 @@ struct PointS float angle; // point orientation PointS( float x = (0.f), float y = (0.f)): - x( x ), y( y ){} + x( x ), y( y ), scale(0.f), angle(0.f){} PointS( const float& x, const float& y,const float& angle,const float& scale): x( x ), y( y ), angle( angle ), scale( scale ){} }; diff --git a/src/openMVG/multiview/projection.cpp b/src/openMVG/multiview/projection.cpp index 2f2db0fada..11123e5a48 100644 --- a/src/openMVG/multiview/projection.cpp +++ b/src/openMVG/multiview/projection.cpp @@ -113,12 +113,8 @@ void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp) { } // Compute translation. - Vec p(3); - p << P(0,3), P(1,3), P(2,3); - Vec3 t = K.inverse() * p; - Eigen::PartialPivLU lu(K); - t = lu.solve(P.col(3)); + Vec3 t = lu.solve(P.col(3)); if(R.determinant()<0) { R = -R; diff --git a/src/openMVG/robust_estimation/robust_estimator_ACRansac_test.cpp b/src/openMVG/robust_estimation/robust_estimator_ACRansac_test.cpp index 5bc96d97f4..10946d93f2 100644 --- a/src/openMVG/robust_estimation/robust_estimator_ACRansac_test.cpp +++ b/src/openMVG/robust_estimation/robust_estimator_ACRansac_test.cpp @@ -76,7 +76,6 @@ class ACRANSACOneViewKernel Mat x1_; Mat3 N1_; double logalpha0_; - bool bPointToLine_; }; // Test ACRANSAC with the AC-adapted Line kernel in a noise/outlier free dataset diff --git a/src/openMVG/split/split.hpp b/src/openMVG/split/split.hpp index ac11715889..18858334be 100644 --- a/src/openMVG/split/split.hpp +++ b/src/openMVG/split/split.hpp @@ -7,7 +7,7 @@ /** * Split an input string with a delimiter and fill a string vector */ -static bool split ( const std::string src, const std::string& delim, std::vector& vec_value ) +static bool split ( const std::string & src, const std::string& delim, std::vector& vec_value ) { bool bDelimiterExist = false; if ( !delim.empty() ) From 2033587ae924de8f69bcb5ab607cfc103bf93634 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sat, 1 Feb 2014 12:44:25 +0100 Subject: [PATCH 44/68] Big update of the doc. Samples and Softwares part. #49 --- docs/sphinx/rst/dependencies/glfw.rst | 4 +- docs/sphinx/rst/dependencies/openExif.rst | 4 +- docs/sphinx/rst/dependencies/osi_clp.rst | 8 +- docs/sphinx/rst/index.rst | 3 +- .../rst/openMVG/linear_programming/lp.rst | 3 + docs/sphinx/rst/openMVG/openMVG.rst | 3 +- .../rst/openMVG_Samples/openMVG_Samples.rst | 78 +++++++- docs/sphinx/rst/patented/patented.rst | 1 + docs/sphinx/rst/software/SfM/MVS.rst | 25 +++ docs/sphinx/rst/software/SfM/SfM.rst | 169 +++++------------- .../rst/software/SfM/geometricMatches.rst | 66 +++++++ docs/sphinx/rst/software/SfM/globalSfM.rst | 69 +++++++ .../rst/software/SfM/incrementalSfM.rst | 82 +++++++++ .../rst/software/SfM/intrinsicGroups.rst | 77 ++++++++ .../rst/software/colorHarmonization/color.rst | 5 + docs/sphinx/rst/software/softwares.rst | 16 ++ docs/sphinx/rst/testing/testing.rst | 6 - 17 files changed, 475 insertions(+), 144 deletions(-) create mode 100644 docs/sphinx/rst/openMVG/linear_programming/lp.rst create mode 100644 docs/sphinx/rst/software/SfM/MVS.rst create mode 100644 docs/sphinx/rst/software/SfM/geometricMatches.rst create mode 100644 docs/sphinx/rst/software/SfM/globalSfM.rst create mode 100644 docs/sphinx/rst/software/SfM/incrementalSfM.rst create mode 100644 docs/sphinx/rst/software/SfM/intrinsicGroups.rst create mode 100644 docs/sphinx/rst/software/colorHarmonization/color.rst create mode 100644 docs/sphinx/rst/software/softwares.rst delete mode 100644 docs/sphinx/rst/testing/testing.rst diff --git a/docs/sphinx/rst/dependencies/glfw.rst b/docs/sphinx/rst/dependencies/glfw.rst index 875fc2e53e..fe2d981b07 100644 --- a/docs/sphinx/rst/dependencies/glfw.rst +++ b/docs/sphinx/rst/dependencies/glfw.rst @@ -8,9 +8,9 @@ USAGE openMVG uses GLFW library in order to provide an easy way to create openGL windows context. -========== +=========== Description -========== +=========== "[GLFW]_ is an Open Source, multi-platform library for creating windows with OpenGL contexts and managing input and events. It is easy to integrate into existing applications and does not lay claim to the main loop. diff --git a/docs/sphinx/rst/dependencies/openExif.rst b/docs/sphinx/rst/dependencies/openExif.rst index b763716761..c509f1c3e3 100644 --- a/docs/sphinx/rst/dependencies/openExif.rst +++ b/docs/sphinx/rst/dependencies/openExif.rst @@ -8,9 +8,9 @@ USAGE openMVG uses OpenEXIF library in order to read JPEG EXIF metadata. -========== +=========== Description -========== +=========== "[OpenExif]_ is an object-oriented library for accessing Exif formatted JPEG image files. The toolkits allows for creating, reading, and modifying the metadata in the Exif file. It also provides mean of getting and setting the main image and the thumbnail image. " diff --git a/docs/sphinx/rst/dependencies/osi_clp.rst b/docs/sphinx/rst/dependencies/osi_clp.rst index 0ad8c0107b..915fb92e2b 100644 --- a/docs/sphinx/rst/dependencies/osi_clp.rst +++ b/docs/sphinx/rst/dependencies/osi_clp.rst @@ -8,13 +8,13 @@ USAGE openMVG uses the [OSI]_ and the [CLP]_ solver in order to solve linear programs [LP]_. -CLP have been choosen because is it known to support problems of up to 1.5 million constraints [CLP FAQ]_. +CLP have been choosen because is it known to support problems of up to 1.5 million constraints [CLP_FAQ]_. [LPSOLVE]_ have been tested but tests shown that it is less reliable (sometimes, there is no convergence to a solution). -========== +=========== Description -========== +=========== [OSI]_ Osi (Open Solver Interface) provides an abstract base class to a generic linear programming (LP) solver, along with derived classes for specific solvers. Many applications may be able to use the Osi to insulate themselves from a specific LP solver. (CLP and MOSEK backend are supported in openMVG). @@ -25,5 +25,5 @@ Description .. [LP] http://en.wikipedia.org/wiki/Linear_programming .. [OSI] https://projects.coin-or.org/Osi .. [CLP] https://projects.coin-or.org/Clp/wiki -.. [CLP FAQ] https://projects.coin-or.org/Clp/wiki/FAQ +.. [CLP_FAQ] https://projects.coin-or.org/Clp/wiki/FAQ .. [LPSOLVE] http://lpsolve.sourceforge.net/5.5/ diff --git a/docs/sphinx/rst/index.rst b/docs/sphinx/rst/index.rst index edb37f50ca..7d199b17db 100644 --- a/docs/sphinx/rst/index.rst +++ b/docs/sphinx/rst/index.rst @@ -48,8 +48,7 @@ Contents: openMVG/openMVG.rst openMVG_Samples/openMVG_Samples.rst patented/patented.rst - software/SfM/SfM.rst - testing/testing.rst + software/softwares.rst dependencies/external_libs.rst third_party/third_party.rst FAQ/FAQ.rst diff --git a/docs/sphinx/rst/openMVG/linear_programming/lp.rst b/docs/sphinx/rst/openMVG/linear_programming/lp.rst new file mode 100644 index 0000000000..6c993405fb --- /dev/null +++ b/docs/sphinx/rst/openMVG/linear_programming/lp.rst @@ -0,0 +1,3 @@ +************************* +Linear programming +************************* diff --git a/docs/sphinx/rst/openMVG/openMVG.rst b/docs/sphinx/rst/openMVG/openMVG.rst index 61a507916a..59bfb94acf 100644 --- a/docs/sphinx/rst/openMVG/openMVG.rst +++ b/docs/sphinx/rst/openMVG/openMVG.rst @@ -1,5 +1,5 @@ ############################ -openMVG +openMVG core ############################ The openMVG library core module @@ -29,4 +29,5 @@ Here the list of libraries that: tracks/tracks.rst robust_estimation/robust_estimation.rst bundle_adjustment/bundle_adjustment.rst + linear_programming/lp.rst diff --git a/docs/sphinx/rst/openMVG_Samples/openMVG_Samples.rst b/docs/sphinx/rst/openMVG_Samples/openMVG_Samples.rst index f9d8d78ba6..8b697801ad 100644 --- a/docs/sphinx/rst/openMVG_Samples/openMVG_Samples.rst +++ b/docs/sphinx/rst/openMVG_Samples/openMVG_Samples.rst @@ -1,6 +1,78 @@ ############################ -openMVG_Samples +openMVG samples ############################ -.. toctree:: - :maxdepth: 2 \ No newline at end of file +openMVG focus on a strong implementation checking of the provided features. +To do so it provides unit test (that assert code results and helps user to see how the code must be used) but it provides also illustrated samples of the major features. + +The samples can be seen as showcase and tutorials: + +imageData +---------- + + - some pictures for each of the following examples. + +siftPutativeMatches +-------------------- + +Show how: + - extract SIFT features and descriptors, + - match features descriptors, + - display the computed matches. + +robust_homography +-------------------- + +Show how: + - estimate a robust homography between features matches. + +robust_homography_guided +--------------------------- +Show how: + - estimate a robust homography between features matches, + - extent the putative matches with a guided filter, + - warp the query image over the reference image. + +robust_fundamental +-------------------- +Show how: + - estimate a robust fundamental matrix between features matches. + +robust_essential +-------------------- +Show how: + - estimate a robust essential matrix between features matches, + - compute the 3D structure by triangulation of the corresponding inliers. + +robust_essential_ba +-------------------- +Show how: + - refine with bundle_adjustment the Structure and Motion of a scene + - for different camera model: + + - Refine ``[X],[f,R|t]`` (individual cameras), + - Refine ``[X],[R|t]``, shared ``[f]``, + - Refine ``[X],[R|t]``, shared brown disto models. + +kvld_filter +-------------------- +Show how: + - filter putative matches with the K-VLD filter. + +exifParsing +-------------------- +Show how: + - parse JPEG EXIF metadata + +sensorWidthDatabase +-------------------- +Show how: + - use the camera sensor width database + +undisto_Brown +-------------------- +Show how: + - undistord a picture according known Brown radial parameters. + + +**Don't hesitate to help to extend the list.** diff --git a/docs/sphinx/rst/patented/patented.rst b/docs/sphinx/rst/patented/patented.rst index 362637cc8e..e7dc47b3f5 100644 --- a/docs/sphinx/rst/patented/patented.rst +++ b/docs/sphinx/rst/patented/patented.rst @@ -15,6 +15,7 @@ The used code is a subset of the [VLFEAT]_ library. You can replace this keypoints and descriptors provided by any version of your choice to use openMVG in a non-research context. Suggestions for features points: + - CORNERS: HARRIS, FAST, - REGIONS: MSER, - BLOBS: AKAZE. diff --git a/docs/sphinx/rst/software/SfM/MVS.rst b/docs/sphinx/rst/software/SfM/MVS.rst new file mode 100644 index 0000000000..84396107bd --- /dev/null +++ b/docs/sphinx/rst/software/SfM/MVS.rst @@ -0,0 +1,25 @@ + +Multiple View Stereovision +==================================== + +Multiple View Stereo-vision +***************************** + +Once camera position and orientation have been computed, Multiple View Stereo-vision algorithms could be used +to compute a dense scene representation. + + +OpenMVG exports [PMVS]_ and [CMPMVS]_ ready to use project (images, projection matrices and pmvs_options.txt files). + +.. figure:: resultOutput.png + :align: center + + Figure: Multiple View Stereo-vision point cloud densification on the estimated scene using [PMVS]_. + +.. [PMVS] Accurate, dense, and robust multi-view stereopsis. + Yasutaka Furukawa and Jean Ponce. + IEEE Trans. on Pattern Analysis and Machine Intelligence, 32(8):1362-1376, 2010. + +.. [CMPMVS] Multi-View Reconstruction Preserving Weakly-Supported Surfaces, M. Jancosek, T. Pajdla, IEEE Conference on Computer Vision and Pattern Recognition 2011. + + diff --git a/docs/sphinx/rst/software/SfM/SfM.rst b/docs/sphinx/rst/software/SfM/SfM.rst index 88cb53bbf5..01a344fbc1 100644 --- a/docs/sphinx/rst/software/SfM/SfM.rst +++ b/docs/sphinx/rst/software/SfM/SfM.rst @@ -1,6 +1,6 @@ -******************************* +*************************** SfM: Structure-from-Motion -******************************* +*************************** Structure from Motion computes an external camera pose per image (the motion) and a 3D point cloud (the structure) representing the pictured scene. Inputs are images and internal camera calibration information (intrinsic parameters). @@ -10,160 +10,81 @@ There are three main approaches to solve the SfM problem: - the hierarchical pipeline, - the global one. -.. figure:: imagesInput.png - :align: center - - Figure : Input images, estimated camera location and structure. - - -openMVG proposes a customizable implementation of an Incremental Structure from Motion chain, it is an evolution of the implementation used for the paper "Adaptive Structure from Motion with a contrario model estimation" [ACSfM]_ published at ACCV 2012. - .. figure:: structureFromMotion.png :align: center - Figure: Structure from Motion illustration, from pictures to 3D. - -The incremental pipeline is a growing reconstruction process (i.e algorithm 2). -It starts from an initial two-view reconstruction (the seed) that is iteratively extended by adding new views and 3D points, using pose estimation and triangulation. -Due to the incremental nature of the process, successive steps of non-linear refinement, like Bundle Adjustment (BA) and Levenberg-Marquardt steps, are performed to minimize the accumulated error (drift). - -The general feature correspondence and SfM processes are described in algorithms 1 and 2. -The first algorithm outputs pairwise correspondences that are consistent with the estimated fundamental matrix. -The initial pair must be choosen with numerous correspondences while keeping a wide enough baseline. -The second algorithm takes these correspondences as input and yields a 3D point cloud as well as the camera poses. -Steps marked with a star (*) are estimated within the a contrario framework. -This allows critical thresholds to be automatically adapted to the input images (and remove the choice of an empiric T threshold value). - -Algorithm 1 Computation of geometry-consistent pairwise correspondences - -.. code-block:: c++ - - Require: image set - Ensure: pairwise point correspondences that are geometrically consistent - Compute putative matches: - detect features in each image and build their descriptor - match descriptors (brute force or approximate nearest neighbor) - Filter geometric-consistent matches: - * estimate fundamental matrix F - -Algorithm 2 Incremental Structure from Motion - -.. code-block:: c++ - - Require: internal camera calibration (matrix K, possibly from EXIF data) - Require: pairwise geometry consistent point correspondences - Ensure: 3D point cloud - Ensure: camera poses - compute correspondence tracks t - compute connectivity graph G (1 node per view, 1 edge when enough matches) - pick an edge e in G with sufficient baseline - * robustly estimate essential matrix from images of e - triangulate validated tracks, which provides an initial reconstruction - contract edge e - while G contains an edge do - pick edge e in G that maximizes union(track(e),3D points) - * robustly estimate pose (external orientation/resection) - triangulate new tracks - contract edge e - perform bundle adjustment - end while - - -Once camera position and orientation have been computed, Multiple View Stereo-vision algorithms could be used -to compute a dense scene representation . -OpenMVG exports a [PMVS]_ ready to use project (images, projection matrices and pmvs_options.txt files). - -.. figure:: resultOutput.png - :align: center - - Figure: Multiple View Stereo-vision point cloud densification on the estimated scene using [PMVS]_. - -Tools to draw detected Keypoints, putative matches, geometric matches and tracks are provided -along the SfM binary directory. - -Structure from Motion chain usage -===================================== + Figure: From point observation and internal knowledge of camera parameter, the 3D **structure** of the scene is computed **from** the estimated **motion** of the camera. -The a contrario Structure from Motion chain take as input an image collection. -Using a 3 directories based data organisation structure is suggested: +In a nutshell +=================== -* images +From an image sequence and an approximated focal length it is possible to compute the following: - - your image sequence. - -* matches +.. figure:: imagesInput.png + :align: center + + Figure : Input images, estimated camera location and structure. - * the image information (lists.txt), images points and matches information will be saved here -* outReconstruction +openMVG tools +==================== - * directory where result and log of the 3D reconstruction will be exported + - 2 **Structure from Motion pipeline**: -1. Intrinsic analysis and export: + - an Incremental Structure from Motion chain [ACSfM]_ (ACCV 2012), + - a Global Structure from Motion chain [GlobalACSfM]_ (ICCV 2013). - The process export in outputDirectory/lists.txt file the extracted camera intrinsics in three way: - - no information can be extracted (only the principal point position is exported) - - the image contain EXIF Jpeg approximated focal length - - if the camera sensor is saved in the openMVG database the focal length and principal point is exported - - else no focal camera can be computed, only the image size is exported. - The focal is computed as follow (with a database of knowed camera sensor size in mm for different camera model): - double ccdw = datasheet._sensorSize; // In mm - focal = max ( width, height ) * focalmm / ccdw; + - **tools** to visualize: - $ ./openMVG_main_CreateList [-i|--imageDirectory] [-d|--sensorWidthDatabase] [-o|--outputDirectory] [-f|--focal] + - features, + - photometric/geometric matches correspondences, + - features tracks. - // Usage of the automatic chain (with JPEG images) - $ ./openMVG_main_CreateList /home/pierre/Pictures/Dataset/images -o /home/pierre/Pictures/Dataset/matches -d ./openMVG/src/software/SfM/cameraSensorWidth/cameraGenerated.txt - // If all the camera have the same focal length and you know it exactly - $ ./openMVG_main_CreateList /home/pierre/Pictures/Dataset/images -o /home/pierre/Pictures/Dataset/matches -f YOURFOCAL(i.e 2750) + - **export to existing Multiple View Stereo-vision pipeline**: -2. Point matching: + - [PMVS]_, CMPMVS. - The first step consists in computing relative image matches (i.e algorithm 2): You have to use the openMVG_main_computeMatches software in the software/SfM openMVG module. + - **tools to build your own SfM pipeline**: - .. code-block:: c++ + - geometric solvers, robust estimators ... - $ openMVG_main_computeMatches -i /home/pierre/Pictures/Dataset/images -o /home/pierre/Pictures/Dataset/matches +To know more about each tool visit the following link and read the doc below: - Arguments are the following: +.. toctree:: + :maxdepth: 1 - - -i|-imadir the path where image are stored. - - -o|-outdir path where features, descriptors, putative and geometric matches will be exported. - - -r|-distratio optional argument (Nearest Neighbor distance ratio, default value is set to 0.6). - - -s|-octminus1 optional argument (Use the octave -1 option of SIFT or not, default value is set to false: 0). + ./intrinsicGroups.rst + ./geometricMatches.rst + ./incrementalSfM.rst + ./globalSfM.rst + ./MVS.rst - Once matches have been computed you can, at your choice, display detected points, matches or - start the 3D reconstruction. -3. Point, matching visualization: +Structure from Motion chain usage +===================================== - Three softwares are available to display: +The a contrario Structure from Motion chain take as input an image collection. - * **Detected keypoints**: openMVG_main_exportKeypoints - * **Putative, Geometric matches**: openMVG_main_exportMatches - * **Tracks**: openMVG_main_exportTracks +Using a 3 directories based data organisation structure is suggested: +* **images** -4. SfM, 3D structure and camera calibration: + - your image sequence. - The main binary in order to run the SfM process is openMVG_main_IncrementalSfM, it use previous - computed data and is implemented as explained in algorithm 2. +* **matches** - .. code-block:: c++ + * the image information (lists.txt), images points and matches information will be saved here - // If you want refine intrinsics (focal, principal point and radial distortion) for each focal group - $ openMVG_main_IncrementalSfM -i /home/pierre/Pictures/Dataset/images/ -m /home/pierre/Pictures/Dataset/matches/ -o /home/pierre/Pictures/Dataset/outReconstruction/ - // If you want only refine the focal (to use with image were the distortion have been already removed) - $ openMVG_main_IncrementalSfM -i /home/pierre/Pictures/Dataset/images/ -m /home/pierre/Pictures/Dataset/matches/ -o /home/pierre/Pictures/Dataset/outReconstruction/ -d 0 +* **outReconstruction** - openMVG_main_IncrementalSfM displays to you some initial pairs that share an important number of common point. - Please select two image indexes that are convergent and the 3D reconstruction will start. + * directory where result and log of the 3D reconstruction will be exported .. [ACSfM] Adaptive structure from motion with a contrario model estimation. Pierre Moulon, Pascal Monasse, and Renaud Marlet. In ACCV, 2012. -.. [PMVS] Accurate, dense, and robust multi-view stereopsis. - Yasutaka Furukawa and Jean Ponce. - IEEE Trans. on Pattern Analysis and Machine Intelligence, 32(8):1362-1376, 2010. +.. [GlobalACSfM] "Global Fusion of Relative Motions for Robust, Accurate and Scalable Structure from Motion." + Pierre Moulon, Pascal Monasse and Renaud Marlet. + In ICCV, 2013. + diff --git a/docs/sphinx/rst/software/SfM/geometricMatches.rst b/docs/sphinx/rst/software/SfM/geometricMatches.rst new file mode 100644 index 0000000000..6e5061e5a8 --- /dev/null +++ b/docs/sphinx/rst/software/SfM/geometricMatches.rst @@ -0,0 +1,66 @@ + +*************************************************************** +Geometric consistent matches for unordered image collection +*************************************************************** + +Computing relative image matches is often required in computer vision. +OpenMVG module provide a binary to compute putative photometric matches and filter them according a choosen robust geometric filter. + +Computation of geometry-consistent pairwise correspondences: +-------------------------------------------------------------- + +.. code-block:: c++ + + Require: image set + Ensure: pairwise geometrically consistent point correspondences + a. Compute putative photometric matches: + detect features in each image and build their descriptor + descriptors matching (brute force or approximate nearest neighbor) + b. Filter geometric-consistent matches: + robust estimation of the choosen geometric model + +In order to compute pairwise geometric consistent matches use the following binary: + + .. code-block:: c++ + + $ openMVG_main_computeMatches [opt. args] -i Dataset/images/ -o Dataset/matches/ + + Arguments are the following: + + - **-i|-imadir** the path where image are stored. + - **-o|-outdir** path where features, descriptors, putative and geometric matches will be exported. + + - Optional arguments + + - **-r|-distratio** optional argument (Nearest Neighbor distance ratio, default value is set to 0.6). + + * -r 0.6 is restrictive, -r 0.8 is less restrictive + + - **-s|-octminus1** optional argument + + - -s 0 Use octave 0 + - -s 1 Use the octave -1 option of SIFT Upscale the image x2. + + - **-p|-peakThreshold** optional argument (Peak threshold for SIFT detection). + + - -p [0.04 (default) to 0.01] range of possible value. 0.01 detects more SIFT keypoints. + + - **-g|-geometricModel** type of model used for robust estimation from the photometric putative matches + + - -g [f(default) or e or h] + - -g f Fundamental matrix filtering + - -g e Essential matrix filtering (all the image must have the same known focal length) + - -g h Homography matrix filtering + + Once matches have been computed you can, at your choice, you can display detected points, matches or + start the 3D reconstruction. + + +Point, matching, tracks visualization: +---------------------------------------- + +* **Detected keypoints**: openMVG_main_exportKeypoints +* **Putative, Geometric matches**: openMVG_main_exportMatches +* **Tracks**: openMVG_main_exportTracks + + diff --git a/docs/sphinx/rst/software/SfM/globalSfM.rst b/docs/sphinx/rst/software/SfM/globalSfM.rst new file mode 100644 index 0000000000..3e36e9fc08 --- /dev/null +++ b/docs/sphinx/rst/software/SfM/globalSfM.rst @@ -0,0 +1,69 @@ + +******************************* +Global Structure from Motion +******************************* + +[GlobalACSfM]_ is based on the paper "Global Fusion of Relative Motions for Robust, Accurate and Scalable Structure from Motion." published at ICCV 2013. + +Multi-view structure from motion (SfM) estimates the position and orientation of pictures in a common 3D coordinate frame. When views are treated incrementally, this external calibration can be subject to drift, contrary to global methods that distribute residual errors evenly. Here the method propose a new global calibration approach based on the fusion of relative motions between image pairs. + +.. code-block:: c++ + + Require: internal camera calibration (possibly from EXIF data) + Require: pairwise geometry consistent point correspondences + Ensure: 3D point cloud + Ensure: camera poses + + compute relative pairwise rotations + detect and remove false relative pairwise rotations + - using composition error of triplet of relative rotations + compute the global rotation + - using a dense least square and approximated rotations + compute relative translations + - using triplet of views for stability and colinear motion support + compute the global translation + - integration of the relative translation directions using a l-∞ method. + final structure and motion + - link tracks validated per triplets and compute global structure by triangulation, + - refine estimated parameter in a 2 step Bundle Adjustment + - refine structure and translations + - refine structure and camera parameters (rotations, translations). + +Information and usage +======================== + +The chain is designed for only one intrinsic group, so all the pictures provided to the chain must have the same focal length. + +The chain is used in a three step process: + 1. Intrinsic analysis, + 2. Geometric features correspondences computation, + 3. Global Structure from Motion. + +1. Intrinsic analysis +----------------------- + +See intrinsic SfM entry. + +2. Geometric features correspondences computation +-------------------------------------------------- + +As approximative or precise focal length is know we can use the essential matrix to filter putative matches. + +.. code-block:: c++ + + $ openMVG_main_computeMatches -g e -p 0.01 -i Dataset/images/ -o Dataset/matches/ + +**Tips** + + As a dense image network is required to perform global SfM it is required to detect more SIFT points per image to ensure a high probability of matching. To have even a more dense image connection graph you can use ``-g e -p 0.01 -r 0.8 -s 1 -i ...`` + +3. Global Structure from Motion +-------------------------------------------------- + +.. code-block:: c++ + + $ openMVG_main_GlobalSfM -i Dataset/images/ -m Dataset/matches/ -o Dataset/outGlobalSfM + + + + diff --git a/docs/sphinx/rst/software/SfM/incrementalSfM.rst b/docs/sphinx/rst/software/SfM/incrementalSfM.rst new file mode 100644 index 0000000000..43e1a2729f --- /dev/null +++ b/docs/sphinx/rst/software/SfM/incrementalSfM.rst @@ -0,0 +1,82 @@ + +************************************* +Incremental Structure from Motion +************************************* + +The [ACSfM]_ SfM is an evolution of the implementation used for the paper "Adaptive Structure from Motion with a contrario model estimation" published at ACCV 2012. + +The incremental pipeline is a growing reconstruction process. +It starts from an initial two-view reconstruction (the seed) that is iteratively extended by adding new views and 3D points, using pose estimation and triangulation. +Due to the incremental nature of the process, successive steps of non-linear refinement, like Bundle Adjustment (BA) and Levenberg-Marquardt steps, are performed to minimize the accumulated error (drift). + +Incremental Structure from Motion + +.. code-block:: c++ + + Require: internal camera calibration (possibly from EXIF data) + Require: pairwise geometry consistent point correspondences + Ensure: 3D point cloud + Ensure: camera poses + compute correspondence tracks t + compute connectivity graph G (1 node per view, 1 edge when enough matches) + pick an edge e in G with sufficient baseline + * robustly estimate essential matrix from images of e + triangulate validated tracks, which provides an initial reconstruction + contract edge e + while G contains an edge do + pick edge e in G that maximizes union(track(e),3D points) + * robustly estimate pose (external orientation/resection) + triangulate new tracks + contract edge e + perform bundle adjustment + end while + +Information and usage +======================== + +The chain is designed for only one intrinsic group, so all the pictures provided to the chain must have the same focal length. + +The chain is used in a three step process: + 1. Intrinsic analysis, + 2. Geometric features correspondences computation, + 3. Global Structure from Motion. + +1. Intrinsic analysis +----------------------- + +See intrinsic SfM entry. + +2. Geometric features correspondences computation +-------------------------------------------------- + +As no intrinsic is supposed to be known the fundamental matrix is used to filter putative matches. + +.. code-block:: c++ + + $ openMVG_main_computeMatches -g f -i Dataset/images/ -o Dataset/matches/ + +Refers to the SfM general help to know more about optional argument to detect more points per images pair. + +**Tips** + + ``-g f -p 0.01 -r 0.8 -i ...`` helps having more matches. + +3. Incremental Structure from Motion +-------------------------------------------------- + +- If you want refine intrinsics (focal, principal point and radial distortion) for each focal group + + .. code-block:: c++ + + $ openMVG_main_IncrementalSfM -i Dataset/images/ -m Dataset/matches/ -o Dataset/outReconstruction/ + +- If you want only refine the focal (to use with image were the distortion have been already removed) + + .. code-block:: c++ + + $ openMVG_main_IncrementalSfM -d 0 -i Dataset/images/ -m Dataset/matches/ -o Dataset/outReconstruction/ + +openMVG_main_IncrementalSfM displays to you some initial pairs that share an important number of common point. + **Please select two image indexes that are convergent and the 3D reconstruction will start. + The initial pair must be choosen with numerous correspondences while keeping a wide enough baseline.** + diff --git a/docs/sphinx/rst/software/SfM/intrinsicGroups.rst b/docs/sphinx/rst/software/SfM/intrinsicGroups.rst new file mode 100644 index 0000000000..c4061cea63 --- /dev/null +++ b/docs/sphinx/rst/software/SfM/intrinsicGroups.rst @@ -0,0 +1,77 @@ + +***************************** +Intrinsic groups +***************************** + +====================================== +openMVG intrinsic group explaination +====================================== + +openMVG groups pictures that share common intrinsic parameter to make parameters estimation more stable. +So each camera will have it's own extrinsic parameter group but can share intrinsic parameters with an image collection. + +Intrinsic image analysis is made from JPEG EXIF metadata and a database of camera sensor width. +If you have image with no metadata you can specify the known pixel focal length value directly. + +Intrinsic analysis and export: +----------------------------------- + +The process exports in outputDirectory/**lists.txt** file the extracted camera intrinsics, one line per image. + + .. code-block:: c++ + + $ cd ./software/SfM/Release/ + $ openMVG_main_CreateList [-i|--imageDirectory] [-d|--sensorWidthDatabase] [-o|--outputDirectory] [-f|--focal] + + - Usage of the automatic chain (with JPEG images) + + .. code-block:: c++ + + $ openMVG_main_CreateList -i Dataset/images/ -o Dataset/matches/ -d ./software/SfM/cameraSensorWidth/cameraGenerated.txt + + + The focal in pixel is computed according the following formula: + + .. math:: + + \text{double ccdw}_\text{mm} = \text{sensorSizeWidth};\\ + \text{focal}_{pix} = max( w_\text{pix}, h_\text{pix} ) * \text{focal}_\text{mm} / \text{ccdw}_\text{mm}; + + - :math:`\text{focal}_{pix}` the EXIF focal length in mm, + - :math:`w_\text{pix}, h_\text{pix}` the image of width and height, + - :math:`\text{ccdw}_\text{mm}` the known sensor size + + - if the EXIF camera model and maker is found the sensorWidthDatabase database openMVG/src/software/SfM/cameraSensorWidth/cameraGenerated.txt + + - If all the camera have the same focal length or no EXIF info, you can set the pixel focal length directly + + .. code-block:: c++ + + $ openMVG_main_CreateList -f 2750(i.e in pixel) -i Dataset/images/ -o Dataset/matches/ + +Depending if EXIF data and if the camera model and make is registered in the camera databset, per lines the following information are displayed: + + - no information can be extracted (only the principal point position is exported) + - the image contain EXIF Jpeg approximated focal length + + - if the camera sensor is saved in the openMVG database the focal length and principal point is exported + - else no focal camera can be computed, only the image size is exported. + + +Example of a lists.txt file where focal is known in advance + +.. code-block:: c++ + + 0000.png;3072;2048;2760;0;1536;0;2760;1024;0;0;1 + 0001.png;3072;2048;2760;0;1536;0;2760;1024;0;0;1 + 0002.png;3072;2048;2760;0;1536;0;2760;1024;0;0;1 + ... + +Example of a lists.txt file where focal is computed from exif data + +.. code-block:: c++ + + 100_7100.JPG;2832;2128;2881.25;EASTMAN KODAK COMPANY;KODAK Z612 ZOOM DIGITAL CAMERA + 100_7101.JPG;2832;2128;2881.25;EASTMAN KODAK COMPANY;KODAK Z612 ZOOM DIGITAL CAMERA + 100_7102.JPG;2832;2128;2881.25;EASTMAN KODAK COMPANY;KODAK Z612 ZOOM DIGITAL CAMERA + ... diff --git a/docs/sphinx/rst/software/colorHarmonization/color.rst b/docs/sphinx/rst/software/colorHarmonization/color.rst new file mode 100644 index 0000000000..3edbea1093 --- /dev/null +++ b/docs/sphinx/rst/software/colorHarmonization/color.rst @@ -0,0 +1,5 @@ +************************ +Color Harmonization +************************ + +TOFill diff --git a/docs/sphinx/rst/software/softwares.rst b/docs/sphinx/rst/software/softwares.rst new file mode 100644 index 0000000000..b73ddb3ef8 --- /dev/null +++ b/docs/sphinx/rst/software/softwares.rst @@ -0,0 +1,16 @@ +############################ +openMVG softwares +############################ + +openMVG provide complete software pipeline +======================================= + +They can perform: + +.. toctree:: + :maxdepth: 1 + + SfM/SfM.rst + colorHarmonization/color.rst + + diff --git a/docs/sphinx/rst/testing/testing.rst b/docs/sphinx/rst/testing/testing.rst deleted file mode 100644 index d3fe1e9c40..0000000000 --- a/docs/sphinx/rst/testing/testing.rst +++ /dev/null @@ -1,6 +0,0 @@ -############################ -testing -############################ - -.. toctree:: - :maxdepth: 2 \ No newline at end of file From dceb7102781afd0ded8b531fe259000e69d04db8 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sat, 1 Feb 2014 19:12:39 +0100 Subject: [PATCH 45/68] Update documentation of the openMVG linear programming module. #49 --- docs/sphinx/rst/conf.py | 4 +- docs/sphinx/rst/index.rst | 24 +-- .../linear_programming/linfinityCV.rst | 36 +++++ .../rst/openMVG/linear_programming/lp.rst | 152 +++++++++++++++++- 4 files changed, 203 insertions(+), 13 deletions(-) create mode 100644 docs/sphinx/rst/openMVG/linear_programming/linfinityCV.rst diff --git a/docs/sphinx/rst/conf.py b/docs/sphinx/rst/conf.py index e4114459e2..0a57749196 100644 --- a/docs/sphinx/rst/conf.py +++ b/docs/sphinx/rst/conf.py @@ -59,9 +59,9 @@ # built documents. # # The short X.Y version. -version = '0.4' +version = '0.5' # The full version, including alpha/beta/rc tags. -release = '0.4' +release = '0.5' # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. diff --git a/docs/sphinx/rst/index.rst b/docs/sphinx/rst/index.rst index 7d199b17db..d71cb7d324 100644 --- a/docs/sphinx/rst/index.rst +++ b/docs/sphinx/rst/index.rst @@ -10,7 +10,7 @@ Introduction ================== OpenMVG (Multiple View Geometry) is a library for computer-vision scientists and especially targeted to the Multiple View Geometry community. It is designed to provide an easy access to the -classical problem solvers in Multiple View Geometry and solve them accurately.. +classical problem solvers in Multiple View Geometry and solve them accurately. ############################ Why another library @@ -28,7 +28,7 @@ openMVG library overview The openMVG library is cut in various modules: -* **Libraries**, +* **Libraries, core**, * comes with unit tests that assert algorithms results and show how use the code. @@ -38,7 +38,11 @@ The openMVG library is cut in various modules: * **Binaries**, - * softwares build to perform toolchain processing (features matching, Structure from Motion ...). + * softwares build to perform toolchain processing + + * features matching in un-ordered photo collection, + * SfM: Structure from Motion, + * color harmonization of photo collection. Contents: @@ -47,20 +51,20 @@ Contents: openMVG/openMVG.rst openMVG_Samples/openMVG_Samples.rst - patented/patented.rst software/softwares.rst + patented/patented.rst dependencies/external_libs.rst third_party/third_party.rst FAQ/FAQ.rst - ############################ Acknowledgements ############################ -openMVG authors would like to thank libmv authors for providing an inspiring base to design the -openMVG library. Authors also would like to thank Mikros Image and LIGM-Imagine laboratory -for support and authorization to make this library as an open-source project. +openMVG authors would like to thank: + +- libmv authors for providing an inspiring base to design the openMVG library. +- Mikros Image and LIGM-Imagine laboratory for support and authorization to make this library as an open-source project. ############################ License @@ -73,8 +77,8 @@ Please refer to the license file contained in the source for complete license de Dependencies ############################ OpenMVG come as a standalone distribution, you don't need to install libraries to make it compiles -and run. Exception for the Linux library is made for the png, zlib and jpeg library in order to use -the one of the system. +and run. +On Linux the library will use if available the local png, zlib and jpeg libraries. Indices and tables ================== diff --git a/docs/sphinx/rst/openMVG/linear_programming/linfinityCV.rst b/docs/sphinx/rst/openMVG/linear_programming/linfinityCV.rst new file mode 100644 index 0000000000..2902cdabd0 --- /dev/null +++ b/docs/sphinx/rst/openMVG/linear_programming/linfinityCV.rst @@ -0,0 +1,36 @@ +######################################################## +L infinity solvers for computer vision +######################################################## + +openMVG propose Linear programming based solver for various problem in computer vision by minimizing of the maximal error the residuals between observations and estimated parameters (The L infinity norm). + +openMVG implements problems introduced by [LinfNorm]_ and generalized by [LinfNormGeneric]. + +Rather than considering quadratic constraints that require SOCP (Second Orde Cone Programming) we consider their LP (linear program) equivalent. It makes usage of residual error expressed with absolute error ( ``|a|=). + +openMVG linear program solvers +--------------------------------- + +openMVG provide access to different solvers (not exhaustive): + +- OSI_CLP (COIN-OR) project, +- MOSEK commercial, free in a research context. + +Those solver have been choosen due to the stability of their results and ability to handle large problems without numerical stability (LPSolve and GPLK have been discarded after extensive experiments). + +I refer the reader to openMVG/src/openMVG/linearProgramming/linear_programming_test.cpp to know more. + +openMVG linear programming module usage +------------------------------------------- + +The linear programming module of openMVG can be used for: + +- solve classical linear problem (optimization), +- test the feasibility of linear problem, +- optimize upper bound of feasible problem (quasi-convex linear programs). + +**classical linear problem solving (optimization)** + +Here an example of usage of the framework: + +.. code-block:: c++ + + // Setup the LP (fill A,b,c and the constraint over x) + LP_Constraints cstraint; + BuildLinearProblem(cstraint); + + // Solve the LP with the solver of your choice + std::vector vec_solution(2); + #if OPENMVG_HAVE_MOSEK + MOSEK_SolveWrapper solver(2); + #else + OSI_CLP_SolverWrapper solver(2); + #endif + // Send constraint to the LP solver + solver.setup(cstraint); + + // If LP have a solution + if (solver.solve()) + // Get back estimated parameters + solver.getSolution(vec_solution); + +**Linear programming, feasible problem** + +openMVG can be use also to test only the feasibility of a given LP + + +.. math:: + + \begin{align} & \text{find} && \mathbf{x}\\ + & \text{subject to} && A \mathbf{x} \leq \mathbf{b} \\ + & \text{and} && \mathbf{x} \ge \mathbf{0} \end{align} + +**Linear programming, quasi convex optimization** + +openMVG used a lot of L infinity minimisation formulation. +Often the posed problems are quasi-convex and dependent of an external parameter that we are looking for (i.e the maximal re-projection error for which the set of contraint is still feasible). + + +Optimization of this upper bound parameter can be done by iterating over all the possible value or by using a bisection that reduce the search range at each iteration. + +.. code-block:: c++ + + Require: gammaLow, gammUp (Low and upper bound of the parameter to optimize) + Require: the LP problem (cstraintBuilder) + Ensure: the optimal gamma value, or return infeasibility of the contraints set. + + BisectionLP( + LP_Solver & solver, + ConstraintBuilder & cstraintBuilder, + double gammaUp = 1.0, // Upper bound + double gammaLow = 0.0, // lower bound + double eps = 1e-8, // precision that stop dichotomy + const int maxIteration = 20) // max number of iteration + { + ConstraintType constraint; + do + { + ++k; // One more iteration + + double gamma = (gammaLow + gammaUp) / 2.0; + + //-- Setup constraint and solver + cstraintBuilder.Build(gamma, constraint); + solver.setup( constraint ); + + //-- Solving + bool bFeasible = solver.solve(); + + //-- According feasibility update the corresponding bound + //-> Feasible, update the upper bound + //-> Not feasible, update the lower bound + (bFeasible) ? gammaUp = gamma; : gammaLow = gamma; + + } while (k < maxIteration && gammaUp - gammaLow > eps); + } + + + +.. [LP] http://en.wikipedia.org/wiki/Linear_programming From a90be308aac6496cf832cd76beb05330c68b98aa Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sat, 1 Feb 2014 19:12:57 +0100 Subject: [PATCH 46/68] Fix a bibliography issue --- .../lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp index 26d09690e1..0b18be3dd4 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp +++ b/src/openMVG/linearProgramming/lInfinityCV/tijsAndXis_From_xi_Ri_noise.hpp @@ -41,8 +41,8 @@ using namespace linearProgramming; // // 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 [1]): -// [1] Outlier Removal Using Duality. +// 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. From 3bf1c1c4fb8e9ff49af70a75bdacaf4d3923031a Mon Sep 17 00:00:00 2001 From: pmoulon Date: Wed, 5 Feb 2014 12:26:18 +0100 Subject: [PATCH 47/68] Crop Exif string to the first NULL character. #75 --- src/openMVG/exif_IO/exif_IO_openExif.hpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/openMVG/exif_IO/exif_IO_openExif.hpp b/src/openMVG/exif_IO/exif_IO_openExif.hpp index ea5d9692db..98c03cdf07 100644 --- a/src/openMVG/exif_IO/exif_IO_openExif.hpp +++ b/src/openMVG/exif_IO/exif_IO_openExif.hpp @@ -34,12 +34,18 @@ class Exif_IO_OpenExif: public Exif_IO std::string getBrand() const { - std::string *s; return (s = getTag(EXIFTAG_MAKE)) == NULL ? "Not found" : *s; + std::string *s = getTag(EXIFTAG_MAKE); + if (s != NULL) + return s->substr(0, strlen(s->c_str())); + return "Not found"; } std::string getModel() const { - std::string *s; return (s = getTag(EXIFTAG_MODEL)) == NULL ? "Not found" : *s; + std::string *s = getTag(EXIFTAG_MODEL); + if (s != NULL) + return s->substr(0, strlen(s->c_str())); + return "Not found"; } std::string getLensModel() const From 19ada5b0157278e3011b6488a2d34b76d5ddca8a Mon Sep 17 00:00:00 2001 From: pmoulon Date: Fri, 7 Feb 2014 16:13:10 +0100 Subject: [PATCH 48/68] Remove Ply files. #78 --- src/software/SfM/CMakeLists.txt | 6 +- src/software/SfMViewer/CMakeLists.txt | 1 - src/software/SfMViewer/Ply.cpp | 585 ---------- src/software/SfMViewer/Ply.h | 1457 ------------------------- src/software/SfMViewer/document.h | 158 +-- 5 files changed, 54 insertions(+), 2153 deletions(-) delete mode 100644 src/software/SfMViewer/Ply.cpp delete mode 100644 src/software/SfMViewer/Ply.h diff --git a/src/software/SfM/CMakeLists.txt b/src/software/SfM/CMakeLists.txt index d8164e9f22..366b0f240e 100644 --- a/src/software/SfM/CMakeLists.txt +++ b/src/software/SfM/CMakeLists.txt @@ -56,13 +56,13 @@ TARGET_LINK_LIBRARIES(openMVG_main_exportTracks # - Export a computed SfM_Ouput scene to PMVS format # -ADD_EXECUTABLE(openMVG_main_openMVG2PMVS main_openMVG2PMVS.cpp ../SfMViewer/Ply.cpp) +ADD_EXECUTABLE(openMVG_main_openMVG2PMVS main_openMVG2PMVS.cpp) TARGET_LINK_LIBRARIES(openMVG_main_openMVG2PMVS ${LIBS}) # - Export a computed SfM_Ouput scene to CMPMVS format # -ADD_EXECUTABLE(openMVG_main_openMVG2CMPMVS main_openMVG2CMPMVS.cpp ../SfMViewer/Ply.cpp) +ADD_EXECUTABLE(openMVG_main_openMVG2CMPMVS main_openMVG2CMPMVS.cpp) TARGET_LINK_LIBRARIES(openMVG_main_openMVG2CMPMVS ${LIBS}) @@ -86,6 +86,6 @@ ENDIF(USE_OPENCV) # - # Export a SfM openMVG scene to meshlab scene with rasters # - -ADD_EXECUTABLE(openMVG_main_openMVG2MESHLAB main_openMVG2MESHLAB.cpp ../SfMViewer/Ply.cpp) +ADD_EXECUTABLE(openMVG_main_openMVG2MESHLAB main_openMVG2MESHLAB.cpp) TARGET_LINK_LIBRARIES(openMVG_main_openMVG2MESHLAB ${LIBS}) diff --git a/src/software/SfMViewer/CMakeLists.txt b/src/software/SfMViewer/CMakeLists.txt index 69628f6385..efb5985cdd 100644 --- a/src/software/SfMViewer/CMakeLists.txt +++ b/src/software/SfMViewer/CMakeLists.txt @@ -26,7 +26,6 @@ INCLUDE_DIRECTORIES( SET(SOURCES main.cpp - Ply.cpp ) ADD_EXECUTABLE(openMVG_main_sfmViewer ${SOURCES} ) diff --git a/src/software/SfMViewer/Ply.cpp b/src/software/SfMViewer/Ply.cpp deleted file mode 100644 index 2e279d20c5..0000000000 --- a/src/software/SfMViewer/Ply.cpp +++ /dev/null @@ -1,585 +0,0 @@ -#include "Ply.h" - -bool Ply::open(const string_type& path, const ElementList& elements, - const ObjInfoList& obj_infos, - const CommentList& comments, - Format format) -{ - _comments.clear(); - _obj_infos.clear(); - _elements.clear(); - - if (format != PLY_FORMAT_ASC - && format != PLY_FORMAT_BIN_BE - && format != PLY_FORMAT_BIN_LE - && format != PLY_FORMAT_UNKNOWN) { - std::cerr << "Ply: unknown format" << std::endl; - return false; - } - - if (format == PLY_FORMAT_UNKNOWN) - format = native_format(); - - if (elements.size() == 0) { - std::cerr << "Ply: no element given" << std::endl; - return false; - } - - for (ElementsIterator it = elements.begin(); - it != elements.end(); ++it) - if (it->num_properties() == 0) { - std::cerr << "Ply: element \"" << it->name() - << "\" has no property" << std::endl; - return false; - } - - _format = format; - _version = 1.0f; - - _comments = comments; - _obj_infos = obj_infos; - _elements = elements; - - _current_element_num = 0; - _current_property_num = 0; - _current_property_list_index = 0; - _current_property_list_count = 0; - - _os.open(path.c_str(), std::ios::out | std::ios::binary); - - if (!_os.is_open()) { - std::cerr << "Ply: can't open file \"" << path << "\"" - << std::endl; - return false; - } - - const int major = int(_version); - const int minor = int(10 * (_version - major)); - - _os << "ply" << std::endl - << "format " << format_enum_to_string(_format) - << ' ' << major << '.' << minor << std::endl; - - for (CommentsIterator it = comments_begin(); - it != comments_end(); ++it) - _os << "comment " << *it << std::endl; - - for (ObjInfosIterator it = obj_infos_begin(); - it != obj_infos_end(); ++it) - _os << "obj_info " << *it << std::endl; - - for (ElementsIterator it = elements_begin(); - it != elements_end(); ++it) { - _os << "element " << it->name() << ' ' - << it->count() << std::endl; - - for (PropertiesIterator it2 = it->properties_begin(); - it2 != it->properties_end(); ++it2) { - const Type type = it2->type(); - - _os << "property " << type_enum_to_string(type) << ' '; - - if (type == PLY_TYPE_LIST) - _os << type_enum_to_string(it2->count_type()) << ' ' - << type_enum_to_string(it2->value_type()) << ' '; - - _os << it2->name() << std::endl; - } - } - - _os << "end_header" << std::endl; - - return (_os != 0); -} - -bool Ply::open(const string_type& path) -{ - _comments.clear(); - _obj_infos.clear(); - _elements.clear(); - - _is.open(path.c_str(), std::ios::in | std::ios::binary); - - if (!_is.is_open()) { - std::cerr << "Ply: can't open file \"" << path << "\"" - << std::endl; - return false; - } - - string_type line_str; - size_type line_num = 0; - - while (getline(_is, line_str)) { - /* Extract tokens */ - std::istringstream iss(line_str); - string_type token; - - if (!(iss >> token)) { - std::cerr << "Ply: can't extract first token on line " - << (line_num + 1) << std::endl; - _is.close(); - return false; - } - - /* The first line must be "ply" */ - if (line_num == 0) { - if (token != "ply") { - std::cerr << "Ply: invalid first line in header" - << std::endl; - _is.close(); - return false; - } - - if (iss >> token) - std::cerr << "Ply: skipping garbage in header" - << std::endl; - - line_num++; - continue; - } - - /* The end of the header */ - if (token == "end_header") { - if (iss >> token) - std::cerr << "Ply: skipping garbage in header" - << std::endl; - - if (!_check_end_of_header()) { - _is.close(); - return false; - } else - break; - } - - if (token == "format") { - if (!_parse_format(iss, line_num)) { - _is.close(); - return false; - } - } else if (token == "comment") { - _parse_comment(iss, line_num); - } else if (token == "obj_info") { - _parse_obj_info(iss, line_num); - } else if (token == "element") { - if (!_parse_element(iss, line_num)) { - _is.close(); - return false; - } - } else if (token == "property") { - if (!_parse_property(iss, line_num)) { - _is.close(); - return false; - } - } else { - std::cerr << "Ply: unsupported token \"" << token - << "\" on line " << (line_num + 1) << std::endl; - _is.close(); - return false; - } - - line_num++; - } - - _current_element_num = 0; - _current_property_num = 0; - _current_property_list_index = 0; - _current_property_list_count = 0; - - return true; -} - -void Ply::close() -{ - if (_os.is_open()) - _os.close(); - - if (_is.is_open()) - _is.close(); -} - -bool Ply::_skip_ASC(const Element& element) -{ - const size_type count = element.count(); - - for (size_type i = 0; i != count; ++i) - for (PropertiesIterator it = element.properties_begin(); - it != element.properties_end(); ++it) { - if (!_property_begin(*it)) - return false; - - if (!_skip_ASC(*it)) { - std::cerr << "Ply: error while skipping property \"" - << it->name() << "\" of element \"" - << element.name() << "\" " << (i + 1) - << std::endl; - return false; - } - - if (!_property_end()) - return false; - } - - return true; -} - -bool Ply::_skip_BIN(const Element& element) -{ - const size_type count = element.count(); - const size_type size = element.size(); - - if (size != 0) { - /* Avoid calling _property_begin()/_property_end() for nothing */ - _current_property_num += element.num_properties(); - - return (_is.seekg(count * size, std::ios::cur) != 0); - } - - for (size_type i = 0; i != count; ++i) - for (PropertiesIterator it = element.properties_begin(); - it != element.properties_end(); ++it) { - if (!_property_begin(*it)) - return false; - - if (!_skip_BIN(*it)) { - std::cerr << "Ply: error while skipping property \"" - << it->name() << "\" of element \"" - << element.name() << "\" " << (i + 1) - << std::endl; - return false; - } - - if (!_property_end()) - return false; - } - - return true; -} - -bool Ply::_skip_ASC(const Property& property) -{ - const Type type = property.type(); - - assert(type != PLY_TYPE_UNKNOWN); - - if (type == PLY_TYPE_LIST) { - const Type count_type = property.count_type(); - - size_type count; - - if (!_type_read_ASC(count_type, _is, count)) - return false; - - if (count == 0) - return true; - - const Type value_type = property.value_type(); - - int dummy; - - for (size_type i = 0; i != count; ++i) - if (!_type_read_ASC(value_type, _is, dummy)) - return false; - } else { - int dummy; - - if (!_type_read_ASC(type, _is, dummy)) - return false; - } - - return true; -} - -bool Ply::_skip_BIN(const Property& property) -{ - const Type type = property.type(); - - assert(type != PLY_TYPE_UNKNOWN); - - if (type == PLY_TYPE_LIST) { - const Type count_type = property.count_type(); - const Type value_type = property.value_type(); - - size_type count = 0; - - _type_read(format(), native_format(), count_type, _is, count); - - const size_type value_type_size = type_size(value_type); - - return (_is.seekg(count * value_type_size, std::ios::cur) != 0); - } else { - const size_type size = type_size(type); - - return (_is.seekg(size, std::ios::cur) != 0); - } -} - -bool Ply::_check_last_element() const -{ - const Element& element = _last_element(); - - if (element.num_properties() == 0) { - std::cerr << "Ply: element \"" << element.name() - << "\" has no property" << std::endl; - return false; - } else - return true; -} - -bool Ply::_check_end_of_header() const -{ - if (format() == PLY_FORMAT_UNKNOWN) { - std::cerr << "Ply: unspecified format" << std::endl; - return false; - } - - if (num_elements() == 0) { - std::cerr << "Ply: no element found" << std::endl; - return false; - } - - if (!_check_last_element()) - return false; - - return true; -} - -bool Ply::_parse_format(std::istringstream& iss, size_type line_num) -{ - if (line_num != 1) { - std::cerr << "Ply: unexpected format token on line " - << (line_num + 1) << std::endl; - return false; - } - - string_type token; - - if (!(iss >> token)) { - std::cerr << "Ply: can't extract format token on line " - << (line_num + 1) << std::endl; - return false; - } - - Format format = format_string_to_enum(token); - - if (format == PLY_FORMAT_UNKNOWN) { - std::cerr << "Ply: unsupported format on line " - << (line_num + 1) << std::endl; - return false; - } - - if ((format == PLY_FORMAT_BIN_BE - || format == PLY_FORMAT_BIN_LE) - && native_format() == PLY_FORMAT_UNKNOWN) { - std::cerr << "Ply: unknown native format" << std::endl; - return false; - } - - float version; - - if (!(iss >> version)) { - std::cerr << "Ply: can't extract version token on line " - << (line_num + 1) << std::endl; - return false; - } - - if (iss >> token) { - std::cerr << "Ply: unexpected token \"" << token - << "\" on line " << (line_num + 1) << std::endl; - return false; - } - - if (version != 1.0f) { - std::cerr << "Ply: unsupported version on line " - << (line_num + 1) << std::endl; - return false; - } - - _format = format; - _version = version; - - return true; -} - -void Ply::_parse_comment(std::istringstream& iss, size_type line_num) -{ - iss >> std::noskipws; - char c; - iss >> c; /* Skip first char */ - - string_type comment; - - while (iss >> c) - comment += c; - - _comments.push_back(comment); -} - -void Ply::_parse_obj_info(std::istringstream& iss, size_type line_num) -{ - iss >> std::noskipws; - char c; - iss >> c; /* Skip first char */ - - string_type obj_info; - - while (iss >> c) - obj_info += c; - - _obj_infos.push_back(obj_info); -} - -bool Ply::_parse_element(std::istringstream& iss, size_type line_num) -{ - if (num_elements() != 0) - if (!_check_last_element()) - return false; - - string_type name; - - if (!(iss >> name)) { - std::cerr << "Ply: can't extract name token on line " - << (line_num + 1) << std::endl; - return false; - } - - if (has_element(name)) { - std::cerr << "Ply: already specified element on line " - << (line_num + 1) << std::endl; - return false; - } - - size_type count; - - if (!(iss >> count)) { - std::cerr << "Ply: can't extract count token on line " - << (line_num + 1) << std::endl; - return false; - } - - string_type token; - - if (iss >> token) { - std::cout << "Ply: unexpected token \"" << token - << "\" on line " << (line_num + 1) << std::endl; - return false; - } - - _elements.push_back(Element(name, count)); - - return true; -} - -bool Ply::_parse_property(std::istringstream& iss, size_type line_num) -{ - if (num_elements() == 0) { - std::cerr << "Ply: property specified w/o element on line " - << (line_num + 1) << std::endl; - return false; - } - - string_type token; - - if (!(iss >> token)) { - std::cerr << "Ply: can't extract type token on line " - << (line_num + 1) << std::endl; - return false; - } - - Type type = type_string_to_enum(token); - - if (type == PLY_TYPE_UNKNOWN) { - std::cerr << "Ply: unsupported type on line " - << (line_num + 1) << std::endl; - return false; - } - - Type count_type = PLY_TYPE_UNKNOWN; - Type value_type = PLY_TYPE_UNKNOWN; - - if (type == PLY_TYPE_LIST) { - if (!(iss >> token)) { - std::cerr << "Ply: can't extract count type token" - " on line " << (line_num + 1) - << std::endl; - return false; - } - - count_type = type_string_to_enum(token); - - if (count_type == PLY_TYPE_UNKNOWN) { - std::cerr << "Ply: unsupported count type on line " - << (line_num + 1) << std::endl; - return false; - } else if (count_type == PLY_TYPE_CHAR - || count_type == PLY_TYPE_SHORT - || count_type == PLY_TYPE_INT - || count_type == PLY_TYPE_FLOAT - || count_type == PLY_TYPE_DOUBLE - || count_type == PLY_TYPE_LIST) { - std::cerr << "Ply: invalid count type (" - << type_enum_to_string(count_type) - << ") on line " << (line_num + 1) << std::endl; - return false; - } - - if (!(iss >> token)) { - std::cerr << "Ply: can't extract value type token" - " on line " << (line_num + 1) - << std::endl; - return false; - } - - value_type = type_string_to_enum(token); - - if (value_type == PLY_TYPE_UNKNOWN) { - std::cerr << "Ply: unsupported value type on line " - << (line_num + 1) << std::endl; - return false; - } else if (value_type == PLY_TYPE_LIST) { - std::cerr << "Ply: invalid value type (list) on line " - << (line_num + 1) << std::endl; - return false; - } - } - - if (!(iss >> token)) { - std::cerr << "Ply: can't extract name token on line " - << (line_num + 1) << std::endl; - return false; - } - - /* Get the last element */ - Element& element = _last_element(); - - if (element.has_property(token)) { - std::cerr << "Ply: element \"" << element.name() - << "\" already has a property \"" << token << '"' - << (line_num + 1) << std::endl; - return false; - } - - bool b; - - if (type == PLY_TYPE_LIST) - b = element.add_property(Property(token, count_type, - value_type)); - else - b = element.add_property(Property(token, type)); - - return b; -} - -void Ply::_find_native_format() -{ - const uint32_type u32 = 0x12345678; - const uint8_type* ptr = reinterpret_cast(&u32); - - if (ptr[0] == 0x12 && ptr[1] == 0x34 - && ptr[2] == 0x56 && ptr[3] == 0x78) - _native_format = PLY_FORMAT_BIN_BE; - else if (ptr[0] == 0x78 && ptr[1] == 0x56 - && ptr[2] == 0x34 && ptr[3] == 0x12) - _native_format = PLY_FORMAT_BIN_LE; - else - _native_format = PLY_FORMAT_UNKNOWN; -} diff --git a/src/software/SfMViewer/Ply.h b/src/software/SfMViewer/Ply.h deleted file mode 100644 index 43b306e6fb..0000000000 --- a/src/software/SfMViewer/Ply.h +++ /dev/null @@ -1,1457 +0,0 @@ -#ifndef PLY_H -#define PLY_H - -#include -#include /* CHAR_BIT */ -#include -#include -#include -#include -#include -#include -#include -#include - -#if CHAR_BIT != 8 -# error 8-bit bytes assumed -#endif /* CHAR_BIT */ - -/*! - * A class to read and write files in PLY (PoLYgon ?) format. - * - * Sample code to read a PLY file: - * - * \code - * Ply ply; - * - * // Read PLY header - * if (!ply.open(path)) { - * // ... - * } - * - * // Iterate over elements - * for (Ply::ElementsIterator it = ply.elements_begin(); - * it != ply.elements_end(); ++it) { - * const Ply::Element& element = *it; - * - * if (element.name() == "some_element_to_skip") { - * if (!ply.skip(element)) - * // ... - * else - * continue; - * } else { - * const size_type& num_elements = element.count(); - * - * for (size_type i = 0; i != num_elements; ++i) { - * for (Ply::PropertiesIterator it2 = - * element.properties_begin(); - * it2 != element.properties_end(); ++it2) { - * const Ply::Property& property = *it2; - * - * if (it2->name() == "some_property_to_read") { - * double d; - * ply.read(*it2, d); - * // ... - * } else { - * if (!ply.skip(*it2)) - * // ... - * else - * continue; - * } - * } - * } - * } - * } - * - * ply.close(); - * \endcode - * - * Sample code to write a PLY file: - * - * \code - * Ply ply; - * - * Ply::ElementList elements; - * - * // ... - * - * // Write PLY header - * if (!ply.open(path, elements, Ply::PLY_FORMAT_ASC)) { - * // ... - * } - * - * // Iterate over elements - * for (Ply::ElementsIterator it = ply.elements_begin(); - * it != ply.elements_end(); ++it) { - * const Ply::Element& element = *it; - * - * const Ply::string_type name = element.name(); - * const size_type& num_elements = element.count(); - * - * for (size_type i = 0; i != num_elements; ++i) { - * for (Ply::PropertiesIterator it2 = - * element.properties_begin(); - * it2 = element.properties_end(); ++it2) { - * const Ply::Property& property = *it2; - * - * if (it2->name() == "some_property_to_read") { - * double d; - * ply.write(*it2, d); - * // ... - * } else - * // ... - * } - * } - * } - * - * ply.close(); - * \endcode - */ -class Ply { - public: - typedef std::size_t size_type; - typedef std::string string_type; - - /* Comments ***********************************************/ - - typedef std::vector CommentList; - typedef CommentList::const_iterator CommentsIterator; - - /* Object informations ************************************/ - - typedef std::vector ObjInfoList; - typedef ObjInfoList::const_iterator ObjInfosIterator; - - /* Format *************************************************/ - - enum Format { - PLY_FORMAT_ASC = 0, - PLY_FORMAT_BIN_BE = 1, - PLY_FORMAT_BIN_LE = 2, - PLY_FORMAT_UNKNOWN = 3, - }; - - /* Type ***************************************************/ - - enum Type { - PLY_TYPE_CHAR = 0, - PLY_TYPE_INT8 = PLY_TYPE_CHAR, - PLY_TYPE_UCHAR = 1, - PLY_TYPE_UINT8 = PLY_TYPE_UCHAR, - PLY_TYPE_SHORT = 2, - PLY_TYPE_INT16 = PLY_TYPE_SHORT, - PLY_TYPE_USHORT = 3, - PLY_TYPE_UINT16 = PLY_TYPE_USHORT, - PLY_TYPE_INT = 4, - PLY_TYPE_INT32 = PLY_TYPE_INT, - PLY_TYPE_UINT = 5, - PLY_TYPE_UINT32 = PLY_TYPE_UINT, - PLY_TYPE_FLOAT = 6, - PLY_TYPE_FLOAT32 = PLY_TYPE_FLOAT, - PLY_TYPE_DOUBLE = 7, - PLY_TYPE_FLOAT64 = PLY_TYPE_DOUBLE, - PLY_TYPE_LIST = 8, - PLY_TYPE_UNKNOWN = 9, - }; - - typedef char int8_type; - typedef unsigned char uint8_type; - typedef short int16_type; - typedef unsigned short uint16_type; - typedef int int32_type; - typedef unsigned int uint32_type; - typedef float float32_type; - typedef double float64_type; - - /* Property ***********************************************/ - - class Property { - public: - /*! Constructs a scalar property. */ - Property(const string_type& name, Type type) : - _name(name), - _type(type), - _count_type(PLY_TYPE_UNKNOWN), - _value_type(PLY_TYPE_UNKNOWN) - { - assert(!name.empty()); - assert(type != PLY_TYPE_LIST); - assert(type != PLY_TYPE_UNKNOWN); - } - - /*! Constructs a list property. */ - Property(const string_type& name, Type count_type, - Type value_type) : - _name(name), - _type(PLY_TYPE_LIST), - _count_type(count_type), - _value_type(value_type) - { - assert(!name.empty()); - assert(count_type != PLY_TYPE_UNKNOWN); - assert(count_type != PLY_TYPE_LIST); - assert(value_type != PLY_TYPE_UNKNOWN); - assert(value_type != PLY_TYPE_LIST); - } - - /*! Returns the name of the property. */ - const string_type& name() const { return _name; } - - /*! Returns the type of the property. */ - Type type() const { return _type; } - - /*! Returns the count type of the property (For lists only)). */ - Type count_type() const - { - assert(type() == PLY_TYPE_LIST); - return _count_type; - } - - /*! Returns the value type of the property (For lists only)). */ - Type value_type() const - { - assert(type() == PLY_TYPE_LIST); - return _value_type; - } - - protected: - /*const*/ string_type _name; - /*const*/ Type _type; - /*const*/ Type _count_type; /* For lists only */ - /*const*/ Type _value_type; /* For lists only */ - }; - - /* Element ************************************************/ - - class Element { - public: - typedef std::vector PropertyList; - typedef PropertyList::const_iterator PropertiesIterator; - - public: - /*! Constructs an element. */ - Element(const string_type& name, size_type count) : - _name(name), - _count(count) { } - - /*! Returns the name of the element. */ - const string_type& name() const { return _name; } - - /*! Returns the count of the element. */ - const size_type& count() const { return _count; } - - /*! - * Returns the size required for this element - * or 0 if the size is variable. - */ - size_type size() const - { - size_type s = 0; - - for (PropertiesIterator it = properties_begin(); - it != properties_end(); ++it) { - const size_type t = type_size(it->type()); - - if (t == 0) { - s = 0; - break; - } - - s += t; - } - - return s; - } - - /* Properties *************************/ - - const PropertyList& properties() const { return _properties; } - - /*! Returns the number of properties of this element. */ - size_type num_properties() const { return _properties.size(); } - - PropertiesIterator properties_begin() const { return _properties.begin(); } - - PropertiesIterator properties_end() const { return _properties.end(); } - - /*! Adds a property to the element. */ - bool add_property(const Property& property) - { - assert(property.type() != PLY_TYPE_UNKNOWN); - assert(!property.name().empty()); - - if (has_property(property.name())) { - std::cerr << "Ply: element \"" << this->name() - << "\" already a property \"" - << property.name() << "\"" << std::endl; - return false; - } - - _properties.push_back(property); - - return true; - } - - const Property& property(size_type i) const { return _properties[i]; } - - PropertiesIterator find_property(const string_type& name) const - { - PropertiesIterator it; - - for (it = properties_begin(); it != properties_end(); ++it) - if (it->name() == name) - return it; - - return it; /* = properties_end() */ - } - - bool has_property(const string_type& name) const { return (find_property(name) != properties_end()); } - - protected: - /* const */ string_type _name; - /* const */ size_type _count; - PropertyList _properties; - }; - - typedef Element::PropertiesIterator PropertiesIterator; - typedef std::vector ElementList; - typedef ElementList::const_iterator ElementsIterator; - - public: - /*! Constructs a new Ply object. */ - Ply() : - _format(PLY_FORMAT_UNKNOWN), - _version(0.0) - { - assert(type_size(PLY_TYPE_INT8) == 1); - assert(type_size(PLY_TYPE_UINT8) == 1); - assert(type_size(PLY_TYPE_INT16) == 2); - assert(type_size(PLY_TYPE_UINT16) == 2); - assert(type_size(PLY_TYPE_INT32) == 4); - assert(type_size(PLY_TYPE_UINT32) == 4); - assert(type_size(PLY_TYPE_FLOAT32) == 4); - assert(type_size(PLY_TYPE_FLOAT64) == 8); - - _find_native_format(); - } - - /* Open/Close *********************************************/ - - /*! Opens the given file and write header information. */ - bool open(string_type path, const ElementList& elements, - Format format = PLY_FORMAT_UNKNOWN) - { - ObjInfoList obj_infos; - CommentList comments; - - return open(path, elements, obj_infos, comments, format); - } - - /*! Opens the given file and write header information. */ - bool open(const string_type& path, const ElementList& elements, - const ObjInfoList& obj_infos, - const CommentList& comments, - Format format = PLY_FORMAT_UNKNOWN); - - /*! Opens the given file and read header information. */ - bool open(const string_type& path); - - /*! Sets the floating-point output precision. */ - void precision(size_type num_digits10) { _os.precision(num_digits10); } - - /*! Sets the floating-point output format to scientific. */ - void scientific() { _os.setf(std::ios::scientific, - std::ios::floatfield); } - - /*! Sets the floating-point output format to fixed. */ - void fixed() { _os.setf(std::ios::scientific, - std::ios::floatfield); } - - void close(); - - /* Skip ***************************************************/ - - /*! Skips the given element. */ - bool skip(const Element& element) - { - if (!_element_begin(element)) - return false; - - if (format() == PLY_FORMAT_ASC) { - if (!_skip_ASC(element)) - return false; - } else { - if (!_skip_BIN(element)) - return false; - } - - return _element_end(); - } - - /*! Skips the given property. */ - bool skip(const Property& property) - { - if (!_property_begin(property)) - return false; - - switch (format()) { - case PLY_FORMAT_ASC: - if (!_skip_ASC(property)) - return false; - break; - case PLY_FORMAT_BIN_BE: - case PLY_FORMAT_BIN_LE: - if (!_skip_BIN(property)) - return false; - break; - default: /* PLY_FORMAT_UNKNOWN */ - assert(0); - return false; - } - - return _property_end(); - } - - /* Read ***************************************************/ - - bool read_begin(const Element& element) { return _element_begin(element); } - - template - bool read(const Property& property, T& value) - { - if (!_property_begin(property)) - return false; - - const Type type = property.type(); - - if (!_type_read(format(), native_format(), type, _is, value)) - return false; - - return _property_end(); - } - - template - bool read_count(const Property& property, T& count) - { - assert(property.type() == PLY_TYPE_LIST); - - if (!_property_begin(property)) - return false; - - const Type count_type = property.count_type(); - - if (!_type_read(format(), native_format(), count_type, _is, count)) - return false; - - _current_property_list_count = count; - _current_property_list_index = 0; - - return true; - } - - template - bool read_value(const Property& property, T& value) - { - assert(property.type() == PLY_TYPE_LIST); - - if (_current_property_list_index >= _current_property_list_count) { - std::cerr << "Ply: reading too many list property values" - << std::endl; - return false; - } - - const Type value_type = property.value_type(); - - if (!_type_read(format(), native_format(), value_type, _is, value)) - return false; - - _current_property_list_index++; - - return true; - } - - bool read_end() { return _element_end(); } - - /* Write **************************************************/ - - bool write_begin(const Element& element) { return _element_begin(element); } - - template - bool write(const Property& property, T value) - { - if (!_property_begin(property)) - return false; - - if (format() == PLY_FORMAT_ASC && _current_property_num != 0) - if ((_os << ' ') == 0) { - std::cerr << "Ply: I/O error" << std::endl; - return false; - } - - const Type type = property.type(); - - if (!_type_write(format(), native_format(), type, _os, value)) - return false; - - return _property_end(); - } - - template - bool write_count(const Property& property, T count) - { - assert(property.type() == PLY_TYPE_LIST); - - if (!_property_begin(property)) - return false; - - if (format() == PLY_FORMAT_ASC && _current_property_num != 0) - if ((_os << ' ') == 0) { - std::cerr << "Ply: I/O error" << std::endl; - return false; - } - - const Type count_type = property.count_type(); - - _current_property_list_count = count; - _current_property_list_index = 0; - - return _type_write(format(), native_format(), count_type, _os, count); - } - - template - bool write_value(const Property& property, T value) - { - assert(property.type() == PLY_TYPE_LIST); - - if (format() == PLY_FORMAT_ASC) - if ((_os << ' ') == 0) { - std::cerr << "Ply: I/O error" << std::endl; - return false; - } - - if (_current_property_list_index >= _current_property_list_count) { - std::cerr << "Ply: writing too many list property values" - << std::endl; - return false; - } - - const Type value_type = property.value_type(); - - if (!_type_write(format(), native_format(), value_type, _os, value)) - return false; - - _current_property_list_index++; - - return true; - } - - bool write_end() - { - if (format() == PLY_FORMAT_ASC) - if ((_os << std::endl) == 0) { - std::cerr << "Ply: I/O error" << std::endl; - return false; - } - - return _element_end(); - } - - /* Format *************************************************/ - - Format native_format() const { return _native_format; } - - Format format() const { return _format; } - - float version() const { return _version; } - - /* Comments ***********************************************/ - - size_type num_comments() const { return _comments.size(); } - - CommentsIterator comments_begin() const { return _comments.begin(); } - - CommentsIterator comments_end() const { return _comments.end(); } - - /* Object information *************************************/ - - size_type num_obj_infos() const { return _obj_infos.size(); } - - ObjInfosIterator obj_infos_begin() const { return _obj_infos.begin(); } - - ObjInfosIterator obj_infos_end() const { return _obj_infos.end(); } - - /* Elements ***********************************************/ - - /*! Returns a for the elements. */ - const ElementList& elements() const { return _elements; } - - size_type num_elements() const { return _elements.size(); } - - /*! Returns an iterator for the first element. */ - ElementsIterator elements_begin() const { return _elements.begin(); } - - /*! Returns a past-the-end iterator for the elements. */ - ElementsIterator elements_end() const { return _elements.end(); } - - const Element& element(size_type i) const { return _elements[i]; } - - /*! Finds an element with the given name. */ - ElementsIterator find_element(const string_type& name) const - { - ElementsIterator it; - - for (it = elements_begin(); it != elements_end(); ++it) - if (it->name() == name) - return it; - - return it; /* = elements_end() */ - } - - /*! Checks whether an element with the given name exists. */ - bool has_element(const string_type& name) const { return (find_element(name) != elements_end()); } - - /*! Returns the element being read. */ - const Element& current_element() const { return element(_current_element_num); } - - /*! Returns the property being read. */ - const Property& current_property() const { return current_element().property(_current_property_num); } - - /* Miscellaneous ******************************************/ - - /*! Converts the given format enum to the appropriate string. */ - static string_type format_enum_to_string(Format format) - { - switch (format) { - case PLY_FORMAT_ASC: - return "ascii"; - case PLY_FORMAT_BIN_BE: - return "binary_big_endian"; - case PLY_FORMAT_BIN_LE: - return "binary_little_endian"; - default: /* PLY_FORMAT_UNKNOWN */ - return "unknown"; - } - } - - /*! Converts the given format string to the appropriate enum. */ - static Format format_string_to_enum(const string_type& s) - { - typedef std::pair FormatStringPair; - - static const FormatStringPair pairs[] = { - FormatStringPair(PLY_FORMAT_ASC , "ascii" ), - FormatStringPair(PLY_FORMAT_BIN_BE, "binary_big_endian" ), - FormatStringPair(PLY_FORMAT_BIN_LE, "binary_little_endian"), - }; - static size_type num_pairs = sizeof(pairs) - / sizeof(FormatStringPair); - - for (size_type i = 0; i != num_pairs; ++i) - if (pairs[i].second == s) - return pairs[i].first; - - return PLY_FORMAT_UNKNOWN; - } - - /*! Returns the size (in bytes) of the type or 0 if it a list. */ - static size_type type_size(Type type) - { - switch (type) { - case PLY_TYPE_INT8: /* PLY_TYPE_CHAR */ - return sizeof(int8_type); - case PLY_TYPE_UINT8: /* PLY_TYPE_UCHAR */ - return sizeof(uint8_type); - case PLY_TYPE_INT16: /* PLY_TYPE_SHORT */ - return sizeof(int16_type); - case PLY_TYPE_UINT16: /* PLY_TYPE_USHORT */ - return sizeof(uint16_type); - case PLY_TYPE_INT32: /* PLY_TYPE_INT */ - return sizeof(int32_type); - case PLY_TYPE_UINT32: /* PLY_TYPE_UINT */ - return sizeof(uint32_type); - case PLY_TYPE_FLOAT32: /* PLY_TYPE_FLOAT */ - return sizeof(float32_type); - case PLY_TYPE_FLOAT64: /* PLY_TYPE_DOUBLE */ - return sizeof(float64_type); - default: /* PLY_TYPE_LIST, PLY_TYPE_UNKNOWN */ - return 0; - } - } - - /*! Converts the given type enum to its string representation. */ - static string_type type_enum_to_string(Type type) - { - switch (type) { - case PLY_TYPE_CHAR: /* PLY_TYPE_INT8 */ - return "char"; - case PLY_TYPE_UCHAR: /* PLY_TYPE_UINT8 */ - return "uchar"; - case PLY_TYPE_SHORT: /* PLY_TYPE_INT16 */ - return "short"; - case PLY_TYPE_USHORT: /* PLY_TYPE_UINT16 */ - return "ushort"; - case PLY_TYPE_INT: /* PLY_TYPE_INT32 */ - return "int"; - case PLY_TYPE_UINT: /* PLY_TYPE_UINT32 */ - return "uint"; - case PLY_TYPE_FLOAT: /* PLY_TYPE_FLOAT32 */ - return "float"; - case PLY_TYPE_DOUBLE: /* PLY_TYPE_FLOAT64 */ - return "double"; - case PLY_TYPE_LIST: - return "list"; - default: /* PLY_TYPE_UNKNOWN */ - return "unknown"; - } - } - - /*! Converts the given type string to the appropriate enum. */ - static Type type_string_to_enum(const string_type& s) - { - typedef std::pair TypeStringPair; - - static const TypeStringPair pairs[] = { - TypeStringPair(PLY_TYPE_CHAR , "char" ), - TypeStringPair(PLY_TYPE_INT8 , "int8" ), - TypeStringPair(PLY_TYPE_UCHAR , "uchar" ), - TypeStringPair(PLY_TYPE_UINT8 , "uint8" ), - TypeStringPair(PLY_TYPE_SHORT , "short" ), - TypeStringPair(PLY_TYPE_INT16 , "int16" ), - TypeStringPair(PLY_TYPE_USHORT , "ushort" ), - TypeStringPair(PLY_TYPE_UINT16 , "uint16" ), - TypeStringPair(PLY_TYPE_INT , "int" ), - TypeStringPair(PLY_TYPE_INT32 , "int32" ), - TypeStringPair(PLY_TYPE_UINT , "uint" ), - TypeStringPair(PLY_TYPE_UINT32 , "uint32" ), - TypeStringPair(PLY_TYPE_FLOAT , "float" ), - TypeStringPair(PLY_TYPE_FLOAT32, "float32"), - TypeStringPair(PLY_TYPE_DOUBLE , "double" ), - TypeStringPair(PLY_TYPE_FLOAT64, "float64"), - TypeStringPair(PLY_TYPE_LIST , "list" ), - }; - static size_type num_pairs = sizeof(pairs) - / sizeof(TypeStringPair); - - for (size_type i = 0; i != num_pairs; ++i) - if (pairs[i].second == s) - return pairs[i].first; - - return PLY_TYPE_UNKNOWN; - } - - protected: - bool _element_begin(const Element& element) - { - if (¤t_element() != &element) { - _current_element_num++; - - if (_current_element_num == num_elements()) { - std::cout << "Ply: no more element to read/write" - << std::endl; - return false; - } - - if (¤t_element() != &element) { - std::cout << "Ply: reading/writing wrong element: \"" - << element.name() << "\" instead of \"" - << current_element().name() << '"' << std::endl; - return false; - } - } - - _current_property_num = 0; - - return true; - } - - bool _element_end() - { - const Element& current_element = this->current_element(); - - const size_type num_properties = current_element.num_properties(); - - if (_current_property_num < num_properties) { - if (current_property().type() == PLY_TYPE_LIST) - return _property_end(); - else { - std::cout << "Ply: reading/writing incomplete \"" - << current_element.name() << "\" element" - << std::endl; - return false; - } - } - - return true; - } - - bool _property_begin(const Property& property) - { - const Property& current_property = this->current_property(); - - if (¤t_property != &property) { - if (current_property.type() == PLY_TYPE_LIST) { - if (!_property_end()) - return false; - } else { - std::cout << "Ply: reading/writing wrong property: \"" - << property.name() << "\" instead of \"" - << current_property.name() << '"' - << std::endl; - return false; - } - } - - _current_property_list_index = 0; - _current_property_list_count = 0; - - return true; - } - - bool _property_end() - { - const Property& current_property = this->current_property(); - - if (current_property.type() == PLY_TYPE_LIST - && _current_property_list_index < _current_property_list_count) { - std::cout << "Ply: reading/writing incomplete \"" - << current_property.name() << "\" list property" - << std::endl; - return false; - } - - _current_property_num++; - - return true; - } - - bool _skip_ASC(const Element& element); - - bool _skip_BIN(const Element& element); - - bool _skip_ASC(const Property& property); - - bool _skip_BIN(const Property& property); - - bool _check_last_element() const; - - bool _check_end_of_header() const; - - bool _parse_format(std::istringstream& iss, size_type line_num); - - void _parse_comment(std::istringstream& iss, size_type line_num); - - void _parse_obj_info(std::istringstream& iss, size_type line_num); - - bool _parse_element(std::istringstream& iss, size_type line_num); - - bool _parse_property(std::istringstream& iss, size_type line_num); - - /*! Returns the first element. */ - const Element& _first_element() const { return *_elements.begin(); } - - /*! Returns the last element. */ - const Element& _last_element() const { return *_elements.rbegin(); } - - /*! Returns the last element. */ - Element& _last_element() { return *_elements.rbegin(); } - - /*! Finds the byte order used by the machine. */ - void _find_native_format(); - - /*! - * Reads one item of the given type and converts it to the - * supplied type. - */ - template - static bool _type_read(Format format, Format native_format, Type type, - std::istream& is, T& t) - { - switch (format) { - case PLY_FORMAT_ASC: - return _type_read_ASC(type, is, t); - case PLY_FORMAT_BIN_BE: - return _type_read_BIN_BE(native_format, type, is, t); - case PLY_FORMAT_BIN_LE: - return _type_read_BIN_LE(native_format, type, is, t); - default: /* PLY_FORMAT_UNKNOWN */ - assert(0); - return false; - } - } - - /*! Converts one item to the supplied type and writes it. */ - template - static bool _type_write(Format format, Format native_format, Type type, - std::ostream& os, T t) - { - switch (format) { - case PLY_FORMAT_ASC: - return _type_write_ASC(type, os, t); - case PLY_FORMAT_BIN_BE: - return _type_write_BIN_BE(native_format, type, os, t); - case PLY_FORMAT_BIN_LE: - return _type_write_BIN_LE(native_format, type, os, t); - default: /* PLY_FORMAT_UNKNOWN */ - assert(0); - return false; - } - } - - /* ASCII **************************************************/ - - template - static bool _type_read_ASC(std::istream& is, T& t) - { - if (!(is >> t)) - return false; - else - return true; - } - - template - static bool _type_read_ASC(Type type, std::istream& is, T& t); - - template - static bool _type_write_ASC(std::ostream& os, T t) - { - if (!(os << t)) - return false; - else - return true; - } - - template - static bool _type_write_ASC(Type type, std::ostream& os, T t); - - /* Binary *************************************************/ - - /* Byte-swapping **********************/ - - template - static void _swab(T& t) - { - const size_type size = sizeof(T); - const size_type half_size = size / 2; - - uint8_type* p = reinterpret_cast(&t); - - for (size_type i = 0; i != half_size; ++i) - std::swap(p[i], p[size - i - 1]); - } - - template - static void _swab(T* p, size_type n) - { - const size_type size = sizeof(T); - const size_type half_size = size / 2; - - uint8_type* q = reinterpret_cast(p); - - for (size_type i = 0; i != n; ++i) - for (size_type j = 0; j != half_size; ++j) - std::swap(q[j], q[size - j - 1]); - } - - /* Binary big-endian ******************/ - - template - static bool _type_read_BIN_BE(Format native_format, - std::istream& is, T& t) - { - const size_type size = sizeof(T); - - char* p = reinterpret_cast(&t); - - if (!is.read(p, size)) - return false; - - if (native_format != PLY_FORMAT_BIN_BE) - _swab(t); - - return true; - } - - template - static bool _type_read_BIN_BE(Format native_format, Type type, - std::istream& is, T& t); - - template - static bool _type_write_BIN_BE(Format native_format, - std::ostream& os, T t) - { - if (native_format != PLY_FORMAT_BIN_BE) - _swab(t); - - const size_type size = sizeof(T); - - const char* p = reinterpret_cast(&t); - - if (!os.write(p, size)) - return false; - - return true; - } - - template - static bool _type_write_BIN_BE(Format native_format, Type type, - std::ostream& os, T t); - - /* Binary little-endian ***************/ - - template - static bool _type_read_BIN_LE(Format native_format, - std::istream& is, T& t) - { - const size_type size = sizeof(T); - - char* p = reinterpret_cast(&t); - - if (!is.read(p, size)) - return false; - - if (native_format != PLY_FORMAT_BIN_LE) - _swab(t); - - return true; - } - - template - static bool _type_read_BIN_LE(Format native_format, Type type, - std::istream& is, T& t); - - template - static bool _type_write_BIN_LE(Format native_format, - std::ostream& os, T t) - { - if (native_format != PLY_FORMAT_BIN_LE) - _swab(t); - - const size_type size = sizeof(T); - - const char* p = reinterpret_cast(&t); - - if (!os.write(p, size)) - return false; - - return true; - } - - template - static bool _type_write_BIN_LE(Format native_format, Type type, - std::ostream& os, T t); - - protected: - Format _format; - Format _native_format; - float _version; - CommentList _comments; - ObjInfoList _obj_infos; - ElementList _elements; - - std::ifstream _is; - std::ofstream _os; - - size_type _current_element_num; - size_type _current_property_num; - size_type _current_property_list_index; - size_type _current_property_list_count; -}; - -template<> -inline bool Ply::_type_read_ASC(std::istream& is, - int8_type& t) -{ -#undef min -#undef max - const int8_type min = std::numeric_limits::min(); - const int8_type max = std::numeric_limits::max(); - - int i; - - if (!(is >> i)) - return false; - - if (i < min || i > max) - return false; - - t = int8_type(i); - - return true; -} - -template<> -inline bool Ply::_type_write_ASC(std::ostream& os, - int8_type t) -{ - return ((os << int32_type(t)) != 0); -} - -template<> -inline bool Ply::_type_read_ASC(std::istream& is, - uint8_type& t) -{ - // const uint8_type min = std::numeric_limits::min(); - const uint8_type max = std::numeric_limits::max(); - - unsigned int u; - - if (!(is >> u)) - return false; - - if (/* u < min || */ u > max) - return false; - - t = uint8_type(u); - - return true; -} - -template<> -inline bool Ply::_type_write_ASC(std::ostream& os, - uint8_type t) -{ - return ((os << uint32_type(t)) != 0); -} - -template -inline bool Ply::_type_read_ASC(Type type, std::istream& is, T& t) -{ - bool b; - - int8_type i8 = 0; - uint8_type u8 = 0; - int16_type i16 = 0; - uint16_type u16 = 0; - int32_type i32 = 0; - uint32_type u32 = 0; - float32_type f32 = 0; - float64_type f64 = 0; - - switch (type) { - case PLY_TYPE_INT8: /* PLY_TYPE_CHAR */ - b = _type_read_ASC(is, i8); - t = T(i8); - break; - case PLY_TYPE_UINT8: /* PLY_TYPE_UCHAR */ - b = _type_read_ASC(is, u8); - t = T(u8); - break; - case PLY_TYPE_INT16: /* PLY_TYPE_SHORT */ - b = _type_read_ASC(is, i16); - t = T(i16); - break; - case PLY_TYPE_UINT16: /* PLY_TYPE_USHORT */ - b = _type_read_ASC(is, u16); - t = T(u16); - break; - case PLY_TYPE_INT32: /* PLY_TYPE_INT */ - b = _type_read_ASC(is, i32); - t = T(i32); - break; - case PLY_TYPE_UINT32: /* PLY_TYPE_UINT */ - b = _type_read_ASC(is, u32); - t = T(u32); - break; - case PLY_TYPE_FLOAT32: /* PLY_TYPE_FLOAT */ - b = _type_read_ASC(is, f32); - t = T(f32); - break; - case PLY_TYPE_FLOAT64: /* PLY_TYPE_DOUBLE */ - b = _type_read_ASC(is, f64); - t = T(f64); - break; - default: /* PLY_TYPE_LIST, PLY_TYPE_UNKNOWN */ - assert(0); - return false; - break; - } - - return b; -} - -template -inline bool Ply::_type_write_ASC(Type type, std::ostream& os, T t) -{ - bool b; - - int8_type i8 = 0; - uint8_type u8 = 0; - int16_type i16 = 0; - uint16_type u16 = 0; - int32_type i32 = 0; - uint32_type u32 = 0; - float32_type f32 = 0; - float64_type f64 = 0; - - switch (type) { - case PLY_TYPE_INT8: /* PLY_TYPE_CHAR */ - i8 = int8_type(t); - b = _type_write_ASC(os, i8); - break; - case PLY_TYPE_UINT8: /* PLY_TYPE_UCHAR */ - u8 = uint8_type(t); - b = _type_write_ASC(os, u8); - break; - case PLY_TYPE_INT16: /* PLY_TYPE_SHORT */ - i16 = int16_type(t); - b = _type_write_ASC(os, i16); - break; - case PLY_TYPE_UINT16: /* PLY_TYPE_USHORT */ - u16 = uint16_type(t); - b = _type_write_ASC(os, u16); - break; - case PLY_TYPE_INT32: /* PLY_TYPE_INT */ - i32 = int32_type(t); - b = _type_write_ASC(os, i32); - break; - case PLY_TYPE_UINT32: /* PLY_TYPE_UINT */ - u32 = uint32_type(t); - b = _type_write_ASC(os, u32); - break; - case PLY_TYPE_FLOAT32: /* PLY_TYPE_FLOAT */ - f32 = float32_type(t); - b = _type_write_ASC(os, f32); - break; - case PLY_TYPE_FLOAT64: /* PLY_TYPE_DOUBLE */ - f64 = float64_type(t); - b = _type_write_ASC(os, f64); - break; - default: /* PLY_TYPE_LIST, PLY_TYPE_UNKNOWN */ - assert(0); - return false; - break; - } - - return b; -} - -template -inline bool Ply::_type_read_BIN_BE(Format native_format, Type type, - std::istream& is, T& t) -{ - bool b; - - int8_type i8 = 0; - uint8_type u8 = 0; - int16_type i16 = 0; - uint16_type u16 = 0; - int32_type i32 = 0; - uint32_type u32 = 0; - float32_type f32 = 0; - float64_type f64 = 0; - - switch (type) { - case PLY_TYPE_INT8: /* PLY_TYPE_CHAR */ - b = _type_read_BIN_BE(native_format, is, i8); - t = T(i8); - break; - case PLY_TYPE_UINT8: /* PLY_TYPE_UCHAR */ - b = _type_read_BIN_BE(native_format, is, u8); - t = T(u8); - break; - case PLY_TYPE_INT16: /* PLY_TYPE_SHORT */ - b = _type_read_BIN_BE(native_format, is, i16); - t = T(i16); - break; - case PLY_TYPE_UINT16: /* PLY_TYPE_USHORT */ - b = _type_read_BIN_BE(native_format, is, u16); - t = T(u16); - break; - case PLY_TYPE_INT32: /* PLY_TYPE_INT */ - b = _type_read_BIN_BE(native_format, is, i32); - t = T(i32); - break; - case PLY_TYPE_UINT32: /* PLY_TYPE_UINT */ - b = _type_read_BIN_BE(native_format, is, u32); - t = T(u32); - break; - case PLY_TYPE_FLOAT32: /* PLY_TYPE_FLOAT */ - b = _type_read_BIN_BE(native_format, is, f32); - t = T(f32); - break; - case PLY_TYPE_FLOAT64: /* PLY_TYPE_DOUBLE */ - b = _type_read_BIN_BE(native_format, is, f64); - t = T(f64); - break; - default: /* PLY_TYPE_LIST, PLY_TYPE_UNKNOWN */ - assert(0); - return false; - break; - } - - return b; -} - -template -inline bool Ply::_type_write_BIN_BE(Format native_format, Type type, - std::ostream& os, T t) -{ - bool b; - - int8_type i8 = 0; - uint8_type u8 = 0; - int16_type i16 = 0; - uint16_type u16 = 0; - int32_type i32 = 0; - uint32_type u32 = 0; - float32_type f32 = 0; - float64_type f64 = 0; - - switch (type) { - case PLY_TYPE_INT8: /* PLY_TYPE_CHAR */ - i8 = int8_type(t); - b = _type_write_BIN_BE(native_format, os, i8); - break; - case PLY_TYPE_UINT8: /* PLY_TYPE_UCHAR */ - u8 = uint8_type(t); - b = _type_write_BIN_BE(native_format, os, u8); - break; - case PLY_TYPE_INT16: /* PLY_TYPE_SHORT */ - i16 = int16_type(t); - b = _type_write_BIN_BE(native_format, os, i16); - break; - case PLY_TYPE_UINT16: /* PLY_TYPE_USHORT */ - u16 = uint16_type(t); - b = _type_write_BIN_BE(native_format, os, u16); - break; - case PLY_TYPE_INT32: /* PLY_TYPE_INT */ - i32 = int32_type(t); - b = _type_write_BIN_BE(native_format, os, i32); - break; - case PLY_TYPE_UINT32: /* PLY_TYPE_UINT */ - u32 = uint32_type(t); - b = _type_write_BIN_BE(native_format, os, u32); - break; - case PLY_TYPE_FLOAT32: /* PLY_TYPE_FLOAT */ - f32 = float32_type(t); - b = _type_write_BIN_BE(native_format, os, f32); - break; - case PLY_TYPE_FLOAT64: /* PLY_TYPE_DOUBLE */ - f64 = float64_type(t); - b = _type_write_BIN_BE(native_format, os, f64); - break; - default: /* PLY_TYPE_LIST, PLY_TYPE_UNKNOWN */ - assert(0); - return false; - break; - } - - return b; -} - -template -inline bool Ply::_type_read_BIN_LE(Format native_format, Type type, - std::istream& is, T& t) -{ - bool b; - - int8_type i8 = 0; - uint8_type u8 = 0; - int16_type i16 = 0; - uint16_type u16 = 0; - int32_type i32 = 0; - uint32_type u32 = 0; - float32_type f32 = 0; - float64_type f64 = 0; - - switch (type) { - case PLY_TYPE_INT8: /* PLY_TYPE_CHAR */ - b = _type_read_BIN_LE(native_format, is, i8); - t = T(i8); - break; - case PLY_TYPE_UINT8: /* PLY_TYPE_UCHAR */ - b = _type_read_BIN_LE(native_format, is, u8); - t = T(u8); - break; - case PLY_TYPE_INT16: /* PLY_TYPE_SHORT */ - b = _type_read_BIN_LE(native_format, is, i16); - t = T(i16); - break; - case PLY_TYPE_UINT16: /* PLY_TYPE_USHORT */ - b = _type_read_BIN_LE(native_format, is, u16); - t = T(u16); - break; - case PLY_TYPE_INT32: /* PLY_TYPE_INT */ - b = _type_read_BIN_LE(native_format, is, i32); - t = T(i32); - break; - case PLY_TYPE_UINT32: /* PLY_TYPE_UINT */ - b = _type_read_BIN_LE(native_format, is, u32); - t = T(u32); - break; - case PLY_TYPE_FLOAT32: /* PLY_TYPE_FLOAT */ - b = _type_read_BIN_LE(native_format, is, f32); - t = T(f32); - break; - case PLY_TYPE_FLOAT64: /* PLY_TYPE_DOUBLE */ - b = _type_read_BIN_LE(native_format, is, f64); - t = T(f64); - break; - default: /* PLY_TYPE_LIST, PLY_TYPE_UNKNOWN */ - assert(0); - return false; - break; - } - - return b; -} - -template -inline bool Ply::_type_write_BIN_LE(Format native_format, Type type, - std::ostream& os, T t) -{ - bool b; - - int8_type i8 = 0; - uint8_type u8 = 0; - int16_type i16 = 0; - uint16_type u16 = 0; - int32_type i32 = 0; - uint32_type u32 = 0; - float32_type f32 = 0; - float64_type f64 = 0; - - switch (type) { - case PLY_TYPE_INT8: /* PLY_TYPE_CHAR */ - i8 = int8_type(t); - b = _type_write_BIN_LE(native_format, os, i8); - break; - case PLY_TYPE_UINT8: /* PLY_TYPE_UCHAR */ - u8 = uint8_type(t); - b = _type_write_BIN_LE(native_format, os, u8); - break; - case PLY_TYPE_INT16: /* PLY_TYPE_SHORT */ - i16 = int16_type(t); - b = _type_write_BIN_LE(native_format, os, i16); - break; - case PLY_TYPE_UINT16: /* PLY_TYPE_USHORT */ - u16 = uint16_type(t); - b = _type_write_BIN_LE(native_format, os, u16); - break; - case PLY_TYPE_INT32: /* PLY_TYPE_INT */ - i32 = int32_type(t); - b = _type_write_BIN_LE(native_format, os, i32); - break; - case PLY_TYPE_UINT32: /* PLY_TYPE_UINT */ - u32 = uint32_type(t); - b = _type_write_BIN_LE(native_format, os, u32); - break; - case PLY_TYPE_FLOAT32: /* PLY_TYPE_FLOAT */ - f32 = float32_type(t); - b = _type_write_BIN_LE(native_format, os, f32); - t = T(f32); - break; - case PLY_TYPE_FLOAT64: /* PLY_TYPE_DOUBLE */ - f64 = float64_type(t); - b = _type_write_BIN_LE(native_format, os, f64); - break; - default: /* PLY_TYPE_LIST, PLY_TYPE_UNKNOWN */ - assert(0); - return false; - break; - } - - return b; -} - -#endif /* PLY_HPP */ diff --git a/src/software/SfMViewer/document.h b/src/software/SfMViewer/document.h index 93bceadb07..d8e919de09 100644 --- a/src/software/SfMViewer/document.h +++ b/src/software/SfMViewer/document.h @@ -9,21 +9,25 @@ #define DOCUMENT #include "openMVG/cameras/PinholeCamera.hpp" +#include "openMVG/tracks/tracks.hpp" using namespace openMVG; -#include "software/SfMViewer/Ply.h" - #include "third_party/stlplus3/filesystemSimplified/file_system.hpp" #include -#include -#include +#include +#include +#include +#include +#include #include struct Document { std::vector _vec_points; std::map > _map_visibility; //Inth camera see the Inth 3D point + tracks::STLMAPTracks _tracks; + std::map _map_camera; std::vector _vec_imageNames; std::map > _map_imageSize; @@ -71,109 +75,49 @@ struct Document bool load(const std::string & spath) { - //-- Check if the required file are present. - _sDirectory = spath; - std::string sDirectoryPly = stlplus::folder_append_separator(_sDirectory) + "clouds"; - if (stlplus::is_file(stlplus::create_filespec(sDirectoryPly,"calib","ply")) - && stlplus::is_file(stlplus::create_filespec(_sDirectory,"views","txt"))) - { - //-- Read the ply file: - Ply ply; - size_t num_vertices = 0; - // Read PLY header - if (ply.open(stlplus::create_filespec(sDirectoryPly,"calib","ply"))) - { - // ... - std::cout << "PLY file opened"; - - // Iterate over elements - for (Ply::ElementsIterator it = ply.elements_begin(); - it != ply.elements_end(); ++it) - { - const Ply::Element& element = *it; - if (element.name() != "vertex") - { - if (!ply.skip(element)) - { - std::cerr << "Cannot skip element \"" << element.name() << '"' << std::endl; - ply.close(); - return false; - } - continue; - } - num_vertices = element.count(); - - //Reserve memory - _vec_points.reserve(3*num_vertices); - - const size_t & num_elements = element.count(); - - for (size_t i = 0; i != num_elements; ++i) - { - float pos[3]; - unsigned char color[3]; - float weight; - std::vector visibility; - - ply.read_begin(element); - for (Ply::PropertiesIterator it2 = - element.properties_begin(); - it2 != element.properties_end(); ++it2) - { - const Ply::Property& property = *it2; - if (property.name() == "x") - ply.read(property, pos[0]); - else if (property.name() == "y") - ply.read(property, pos[1]); - else if (property.name() == "z") - ply.read(property, pos[2]); - else if (property.name() == "red") - ply.read(property, color[0]); - else if (property.name() == "green") - ply.read(property, color[1]); - else if (property.name() == "blue") - ply.read(property, color[2]); - else if (property.name() == "weight" || property.name() == "confidence") - ply.read(property, weight); - else if (property.name() == "visibility") - { - size_t visibility_count; - ply.read_count(property, visibility_count); - visibility.reserve(visibility_count); - while (visibility_count--) - { - int visibility_value; - ply.read_value(property, visibility_value); - visibility.push_back(visibility_value); - _map_visibility[visibility_value].push_back(i); //Jnth camera see the Inth point - } - } - else if (!ply.skip(property)) - { - std::cerr << "Cannot skip property \"" << property.name() << '"' << std::endl; - ply.close(); - return false; - } - } - ply.read_end(); - - _vec_points.push_back(pos[0]); - _vec_points.push_back(pos[1]); - _vec_points.push_back(pos[2]); - - /*std::cout << '\n' - << pos[0] <<' ' << pos[1] << ' ' << pos[2] << ' '; - using namespace std; - std::copy(visibility.begin(), visibility.end(), ostream_iterator(std::cout, " ")); - */ - } - } - } - ply.close(); - } - else - { - std::cerr << "Required file(s) is missing" << std::endl; + //-- Check if the required file are present. + _sDirectory = spath; + std::string sDirectoryPly = stlplus::folder_append_separator(_sDirectory) + "clouds"; + if (stlplus::is_file(stlplus::create_filespec(sDirectoryPly,"visibility","txt")) + && stlplus::is_file(stlplus::create_filespec(_sDirectory,"views","txt"))) + { + // Read visibility file (3Dpoint, NbVisbility, [(imageId, featId); ... ) + std::ifstream iFilein(stlplus::create_filespec(sDirectoryPly,"visibility","txt").c_str()); + if (iFilein.is_open()) + { + size_t trackId = 0; + while (!iFilein.eof()) + { + // read one line at a time + std::string temp; + std::getline(iFilein, temp); + std::stringstream sStream(temp); + float pt[3]; + sStream >> pt[0] >> pt[1] >> pt[2]; + int count; + sStream >> count; + size_t imaId, featId; + for (int i = 0; i < count; ++i) + { + sStream >> imaId >> featId; + _tracks[trackId].insert(std::make_pair(imaId, featId)); + _map_visibility[imaId].push_back(trackId); //imaId camera see the point indexed trackId + } + + _vec_points.push_back(pt[0]); + _vec_points.push_back(pt[1]); + _vec_points.push_back(pt[2]); + trackId++; + } + } + else + { + std::cerr << "Cannot open the visibility file" << std::endl; + } + } + else + { + std::cerr << "Required file(s) is missing" << std::endl; } // Read cameras From b0f6110882815ea23b59ea6043307e03e1f4039d Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 9 Feb 2014 17:37:17 +0100 Subject: [PATCH 49/68] Test travis for continuous integration. --- .travis.yml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000000..2faef202ad --- /dev/null +++ b/.travis.yml @@ -0,0 +1,19 @@ +language: cpp + +compiler: + - gcc + +install: sudo apt-get install libpng-dev libjpeg8-dev libxxf86vm1 libxxf86vm1 libxxf86vm-dev x11proto-xf86vidmode-dev libxrandr-dev + +script: + - git clean -f + - git submodule update --init --recursive + - cd build + - cmake -DCMAKE_BUILD_TYPE=release . ../src + - make + - make test + +branches: + only: + - develop + From 1a35b30fc116c71cd7b06807ab78f2de60e4ec20 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 9 Feb 2014 17:48:36 +0100 Subject: [PATCH 50/68] update travis command --- .travis.yml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 2faef202ad..9ad3a5640e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,13 +3,14 @@ language: cpp compiler: - gcc -install: sudo apt-get install libpng-dev libjpeg8-dev libxxf86vm1 libxxf86vm1 libxxf86vm-dev x11proto-xf86vidmode-dev libxrandr-dev +install: sudo apt-get install cmake libpng-dev libjpeg8-dev libxxf86vm1 libxxf86vm1 libxxf86vm-dev x11proto-xf86vidmode-dev libxrandr-dev -script: - - git clean -f - - git submodule update --init --recursive +before_script: + - mkdir build - cd build - - cmake -DCMAKE_BUILD_TYPE=release . ../src + - cmake -DCMAKE_BUILD_TYPE=release . ../openMVG/src + +script: - make - make test From 66e1dd21eaccffcfbc9ceed3702c5a01f4468066 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 9 Feb 2014 17:50:28 +0100 Subject: [PATCH 51/68] update travis command --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 9ad3a5640e..b8c7213a85 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,7 +8,7 @@ install: sudo apt-get install cmake libpng-dev libjpeg8-dev libxxf86vm1 libxxf86 before_script: - mkdir build - cd build - - cmake -DCMAKE_BUILD_TYPE=release . ../openMVG/src + - cmake -DCMAKE_BUILD_TYPE=release . ../openMVG/openMVG/src script: - make From 499ee8b754de34b6b10f48600a25de7a4b3b2c72 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 9 Feb 2014 17:54:00 +0100 Subject: [PATCH 52/68] update travis command --- .travis.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index b8c7213a85..6d9cb2ee31 100644 --- a/.travis.yml +++ b/.travis.yml @@ -6,9 +6,10 @@ compiler: install: sudo apt-get install cmake libpng-dev libjpeg8-dev libxxf86vm1 libxxf86vm1 libxxf86vm-dev x11proto-xf86vidmode-dev libxrandr-dev before_script: + - cd .. - mkdir build - cd build - - cmake -DCMAKE_BUILD_TYPE=release . ../openMVG/openMVG/src + - cmake -DCMAKE_BUILD_TYPE=release . ../openMVG/src script: - make From d6369025948e40376e1050a5404ed4372b5092a4 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 9 Feb 2014 18:09:55 +0100 Subject: [PATCH 53/68] Update status of CI --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 2dad2c5db7..776a7b31d5 100644 --- a/README.md +++ b/README.md @@ -20,9 +20,9 @@ Building See [BUILD](https://github.com/openMVG/openMVG/raw/master/BUILD) text file -Continuous integration (master, develop branch, linux 64 bits): +Continuous integration (develop branch, linux 64 bits, BUILD + UNIT TESTING): -[![Build Status](https://drone.io/github.com/openMVG/openMVG/status.png)](https://drone.io/github.com/openMVG/openMVG/latest) +[![Build Status](https://travis-ci.org/openMVG/openMVG.png?branch=develop)](https://travis-ci.org/openMVG/openMVG) ------- License From 94a2ac44bd2ea563e99fc3540120345cb124e66a Mon Sep 17 00:00:00 2001 From: pmoulon Date: Wed, 12 Feb 2014 08:09:21 +0100 Subject: [PATCH 54/68] Make Vec6 aligned on x86 win32 platforms. #4 --- src/openMVG/numeric/numeric.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/openMVG/numeric/numeric.h b/src/openMVG/numeric/numeric.h index 46fbe90194..b601cb28dc 100644 --- a/src/openMVG/numeric/numeric.h +++ b/src/openMVG/numeric/numeric.h @@ -55,7 +55,6 @@ namespace openMVG { typedef Eigen::Vector2i Vec2i; typedef Eigen::Vector2f Vec2f; typedef Eigen::Vector3f Vec3f; - typedef Eigen::Matrix Vec6; typedef Eigen::Matrix Vec9; typedef Eigen::Quaternion Quaternion; @@ -63,7 +62,7 @@ namespace openMVG { typedef Eigen::Matrix Mat3; #if defined(_WIN32) || defined(WIN32) - // Handle alignment issue with Mat34, Vec2, Vec4 on win32 with old compiler + // Handle alignment issue with Mat34, Vec2, Vec4, Vec6 on win32 with old compiler enum { NeedsToAlignMat34 = (sizeof(Eigen::Matrix)%16)==0 }; typedef Eigen::Matrix Mat34; @@ -72,10 +71,14 @@ namespace openMVG { enum { NeedsToAlignVec4= (sizeof(Eigen::Vector4d)%16)==0 }; typedef Eigen::Matrix Vec4; + + enum { NeedsToAlignVec6= (sizeof(Eigen::Matrix)%16)==0 }; + typedef Eigen::Matrix Vec6; #else // defined(_WIN32) || defined(WIN32) typedef Eigen::Matrix Mat34; typedef Eigen::Vector2d Vec2; typedef Eigen::Vector4d Vec4; + typedef Eigen::Matrix Vec6; #endif // defined(_WIN32) || defined(WIN32) typedef Eigen::Matrix Mat4; From f90b5a8e5d223d2e2fa0d846b0fb03b05200a537 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Wed, 12 Feb 2014 08:14:00 +0100 Subject: [PATCH 55/68] USE_OPENMP is a cmake option. SSE2 preprocessor is added if detected on WIN32 platform. #86 --- src/CMakeLists.txt | 32 +++++++++++++++++++++----------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 69d981ae19..cb8f631098 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -9,6 +9,9 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) PROJECT(openMVG C CXX) +# Multithreading using OpenMP +OPTION(USE_OPENMP "Enable OpenMP thread" ON) + # By default build in Release mode IF(NOT CMAKE_BUILD_TYPE) SET(CMAKE_BUILD_TYPE "Release") @@ -26,23 +29,30 @@ SET(CMAKE_MODULE_PATH INCLUDE(OptimizeForArchitecture) OptimizeForArchitecture() IF (SSE2_FOUND) + IF (MSVC AND NOT ${CMAKE_CL_64}) + ADD_DEFINITIONS(/arch:SSE2) + ENDIF (MSVC AND NOT ${CMAKE_CL_64}) ADD_DEFINITIONS(-DUSE_SSE) ENDIF (SSE2_FOUND) # ============================================================================== # OpenMP detection # ============================================================================== -FIND_PACKAGE(OpenMP) -IF (OPENMP_FOUND) - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - OPTION(USE_OPENMP "Use OpenMP for parallelization" ON) - ADD_DEFINITIONS(-DUSE_OPENMP) - IF (NOT MSVC) - LIST(APPEND OPENMVG_LIBRARY_DEPENDENCIES gomp) - ENDIF (NOT MSVC) -ELSE (OPENMP_FOUND) - OPTION(USE_OPENMP "Use OpenMP for parallelization" OFF) -ENDIF (OPENMP_FOUND) +IF (USE_OPENMP) + FIND_PACKAGE(OpenMP) + IF (OPENMP_FOUND) + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + OPTION(USE_OPENMP "Use OpenMP for parallelization" ON) + ADD_DEFINITIONS(-DUSE_OPENMP) + IF (NOT MSVC) + LIST(APPEND OPENMVG_LIBRARY_DEPENDENCIES gomp) + ENDIF (NOT MSVC) +ELSE (USE_OPENMP) + OPTION(USE_OPENMP "Use OpenMP for parallelization" OFF) + SET(USE_OPENMP OFF CACHE BOOL "Use OpenMP for parallelization" FORCE) + REMOVE_DEFINITIONS(-DUSE_OPENMP) + ENDIF (OPENMP_FOUND) +ENDIF (USE_OPENMP) # ============================================================================== # IMAGE IO detection From e6290ea32d63e03f6d90cac10a490af948ec3927 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sat, 15 Feb 2014 15:25:11 +0100 Subject: [PATCH 56/68] export visibility point cloud for the global SfM chain too. #89 --- src/software/SfM/SfMReconstructionData.hpp | 14 +++++----- src/software/globalSfM/SfMGlobalEngine.cpp | 32 ++++++++++++++++------ src/software/globalSfM/main_GlobalSfM.cpp | 17 ++++++++---- 3 files changed, 43 insertions(+), 20 deletions(-) diff --git a/src/software/SfM/SfMReconstructionData.hpp b/src/software/SfM/SfMReconstructionData.hpp index 2829b5065b..0f8f2aff61 100644 --- a/src/software/SfM/SfMReconstructionData.hpp +++ b/src/software/SfM/SfMReconstructionData.hpp @@ -267,10 +267,10 @@ struct reconstructorHelper Vec3 pos = map_3DPoints.find(trackId)->second; f_cloud << pos.transpose() << " " << "255 255 255" << " " << 3.14; - - - ostringstream s_visibility; - + + + std::ostringstream s_visibility; + std::set< size_t > set_imageIndex; for( tracks::submapTrack::const_iterator iterTrack = track.begin(); iterTrack != track.end(); @@ -286,15 +286,15 @@ struct reconstructorHelper znear[map_cameratoIndex[imageId]] = std::min(znear[map_cameratoIndex[imageId]], z ); zfar[map_cameratoIndex[imageId]] = std::max(zfar[map_cameratoIndex[imageId]], z ); } - + s_visibility << iterTrack->first << " " << iterTrack->second << " "; } - + //export images indexes f_cloud << " " << set_imageIndex.size() << " "; copy(set_imageIndex.begin(), set_imageIndex.end(), std::ostream_iterator(f_cloud, " ")); f_cloud << std::endl; - + f_visibility << pos.transpose() << " " << set_imageIndex.size() << " "; f_visibility << s_visibility.str() << std::endl; } diff --git a/src/software/globalSfM/SfMGlobalEngine.cpp b/src/software/globalSfM/SfMGlobalEngine.cpp index 78faf9f5f5..fe6f58982f 100644 --- a/src/software/globalSfM/SfMGlobalEngine.cpp +++ b/src/software/globalSfM/SfMGlobalEngine.cpp @@ -742,12 +742,21 @@ bool GlobalReconstructionEngine::Process() // Cloud std::ofstream f( stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "clouds", - "calib", "ply").c_str()); + "calib", "ply").c_str()); + std::ofstream f_visibility( + stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "clouds", + "visibility", "txt").c_str()); if (!f.is_open()) { std::cerr << "cannot save cloud" << std::endl; return false; - } + } + + if (!f_visibility.is_open()) { + std::cerr << "cannot save cloud desc" << std::endl; + return false; + } + f << "ply\nformat ascii 1.0\ncomment generated by the Global OpenMVG Calibration Engine" << "\n"; f << "element vertex " << vec_allScenes.size() << "\n"; f << "property float x\nproperty float y\nproperty float z" << "\n"; @@ -762,10 +771,11 @@ bool GlobalReconstructionEngine::Process() // Look through the track and add point position const tracks::submapTrack & track = iter->second; - Vec3 pos = vec_allScenes[i]; + const Vec3 & pos = vec_allScenes[i]; f << pos.transpose() << " " << "255 255 255" << " " << 3.14; - + + std::ostringstream s_visibility; std::set< size_t > set_imageIndex; for( tracks::submapTrack::const_iterator iterTrack = track.begin(); iterTrack != track.end(); @@ -780,15 +790,21 @@ bool GlobalReconstructionEngine::Process() double z = Depth(cam._R, cam._t, pos); znear[map_cameratoIndex[imageId]] = std::min(znear[map_cameratoIndex[imageId]], z ); zfar[map_cameratoIndex[imageId]] = std::max(zfar[map_cameratoIndex[imageId]], z ); - } - } + } + s_visibility << iterTrack->first << " " << iterTrack->second << " "; + } + //export images indexes f << " " << set_imageIndex.size() << " "; copy(set_imageIndex.begin(), set_imageIndex.end(), std::ostream_iterator(f, " ")); - f << std::endl; + f << std::endl; + + f_visibility << pos.transpose() << " " << set_imageIndex.size() << " "; + f_visibility << s_visibility.str() << std::endl; } - f.close(); + f.close(); + f_visibility.close(); // Views f.open(stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory), diff --git a/src/software/globalSfM/main_GlobalSfM.cpp b/src/software/globalSfM/main_GlobalSfM.cpp index 3562555d9d..af06329378 100644 --- a/src/software/globalSfM/main_GlobalSfM.cpp +++ b/src/software/globalSfM/main_GlobalSfM.cpp @@ -16,11 +16,18 @@ using namespace openMVG; int main(int argc, char **argv) { using namespace std; - std::cout << "Global Structure from Motion :" - << " open Source implementation of: \n" - << "\"Global Fusion of Relative Motions for Robust, Accurate and Scalable Structure from Motion.\"" - << " ICCV 2013 paper. Pierre Moulon, Pascal Monasse and Renaud Marlet." << std::endl - << std::endl; + std::cout << std::endl + << "-----------------------------------------------------------\n" + << "Global Structure from Motion:\n" + << "-----------------------------------------------------------\n" + << "Open Source implementation of the paper:\n" + << "\"Global Fusion of Relative Motions for " + << "Robust, Accurate and Scalable Structure from Motion.\"\n" + << "Pierre Moulon, Pascal Monasse and Renaud Marlet. " + << " ICCV 2013." << std::endl + << "------------------------------------------------------------" + << std::endl; + CmdLine cmd; From c6212e4114b12143a5e9804aec1519e834cd82da Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 17 Feb 2014 08:41:37 +0100 Subject: [PATCH 57/68] Refine camera pose once it have been found by AC-RANSAC. #91 --- src/openMVG/cameras/BrownPinholeCamera.hpp | 2 +- src/software/SfM/SfMRobust.hpp | 116 ++++++++++++++++++++- 2 files changed, 116 insertions(+), 2 deletions(-) diff --git a/src/openMVG/cameras/BrownPinholeCamera.hpp b/src/openMVG/cameras/BrownPinholeCamera.hpp index 6d881d249b..c914e12a2c 100644 --- a/src/openMVG/cameras/BrownPinholeCamera.hpp +++ b/src/openMVG/cameras/BrownPinholeCamera.hpp @@ -129,7 +129,7 @@ struct BrownPinholeCamera (cam1._K.inverse() * Vec3(x1(0), x1(1), 1.))).normalized(); Vec3 ray2 = (cam2._R.transpose() * (cam2._K.inverse() * Vec3(x2(0), x2(1), 1.))).normalized(); - double mag = ray1.norm() * ray2.norm(); + double mag = std::sqrt(ray1.squaredNorm() * ray2.squaredNorm()); double dotAngle = ray1.dot(ray2); return R2D(acos(clamp(dotAngle/mag, -1.0 + 1.e-8, 1.0 - 1.e-8))); } diff --git a/src/software/SfM/SfMRobust.hpp b/src/software/SfM/SfMRobust.hpp index 2ad3e9ad4a..6650d8528d 100644 --- a/src/software/SfM/SfMRobust.hpp +++ b/src/software/SfM/SfMRobust.hpp @@ -22,6 +22,9 @@ #include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp" #include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp" +#include "openMVG/bundle_adjustment/problem_data_container.hpp" +#include "openMVG/bundle_adjustment/pinhole_ceres_functor.hpp" + namespace openMVG{ namespace SfMRobust{ @@ -257,9 +260,120 @@ bool robustResection( { // Re-estimate the model from the inlier data // and/or LM to Refine f R|t + + Mat3 K_, R_; + Vec3 t_; + KRt_From_P(*P, &K_, &R_, &t_); + + using namespace openMVG::bundle_adjustment; + // Setup a BA problem + BA_Problem_data<7> ba_problem; + + // Configure the size of the problem + ba_problem.num_cameras_ = 1; + ba_problem.num_points_ = pvec_inliers->size(); + ba_problem.num_observations_ = pvec_inliers->size(); + + ba_problem.point_index_.reserve(ba_problem.num_observations_); + ba_problem.camera_index_.reserve(ba_problem.num_observations_); + ba_problem.observations_.reserve(2 * ba_problem.num_observations_); + + ba_problem.num_parameters_ = 7 * ba_problem.num_cameras_; + ba_problem.parameters_.reserve(ba_problem.num_parameters_); + + double ppx = K_(0,2); + double ppy = K_(1,2); + // Fill it with data (tracks and points coords) + for (int i = 0; i < ba_problem.num_points_; ++i) { + // Collect the image of point i in each frame. + + ba_problem.camera_index_.push_back(0); + ba_problem.point_index_.push_back(i); + const Vec2 & pt = pt2D.col((*pvec_inliers)[i]); + ba_problem.observations_.push_back( pt(0) - ppx ); + ba_problem.observations_.push_back( pt(1) - ppy ); + } + + // Add camera parameters (R, t, focal) + { + // Rotation matrix to angle axis + std::vector angleAxis(3); + ceres::RotationMatrixToAngleAxis((const double*) R_.data(), &angleAxis[0]); + ba_problem.parameters_.push_back(angleAxis[0]); + ba_problem.parameters_.push_back(angleAxis[1]); + ba_problem.parameters_.push_back(angleAxis[2]); + ba_problem.parameters_.push_back(t_[0]); + ba_problem.parameters_.push_back(t_[1]); + ba_problem.parameters_.push_back(t_[2]); + ba_problem.parameters_.push_back(K_(0,0)); //focal + } + + // Create residuals for each observation in the bundle adjustment problem. The + // parameters for cameras and points are added automatically. + ceres::Problem problem; + const double * pt3D_Array = pt3D.data(); + for (int i = 0; i < ba_problem.num_points_; ++i) { + // Each Residual block takes a point and a camera as input and outputs a 2 + // dimensional residual. Internally, the cost function stores the observed + // image location and compares the reprojection against the observation. + + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction( + new pinhole_reprojectionError::ErrorFunc_Refine_Camera( + & ba_problem.observations()[2 * i], + pt3D.col((*pvec_inliers)[i]).data())); + + problem.AddResidualBlock(cost_function, + NULL, // squared loss + ba_problem.mutable_camera_for_observation(0)); + } + + ceres::Solver::Options options; + options.linear_solver_type = ceres::SPARSE_SCHUR; + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::SUITE_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; + else + if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::CX_SPARSE)) + options.sparse_linear_algebra_library_type = ceres::CX_SPARSE; + else + { + // No sparse back end for Ceres. + // Use dense solving + options.linear_solver_type = ceres::DENSE_SCHUR; + } + options.minimizer_progress_to_stdout = false; + options.logging_type = ceres::SILENT; +#ifdef USE_OPENMP + options.num_threads = omp_get_num_threads(); +#endif // USE_OPENMP + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // If no error, get back refined parameters + if (summary.termination_type != ceres::DID_NOT_RUN && + summary.termination_type != ceres::USER_ABORT && + summary.termination_type != ceres::NUMERICAL_FAILURE) + { + const double * Rtf = ba_problem.mutable_camera_for_observation(0); + + // angle axis to rotation matrix + ceres::AngleAxisToRotationMatrix(Rtf, R_.data()); + t_ = Vec3(Rtf[3], Rtf[4], Rtf[5]); + double focal = Rtf[6]; + + Mat3 KRefined; + KRefined << focal,0, ppx, + 0, focal, ppy, + 0, 0, 1; + + PinholeCamera camRefined(KRefined,R_,t_); + *P = camRefined._P; + } + return true; } - else{ + else { P = NULL; return false; } From bfcd7d892ae695295da9abd3616b3487cbc1b45c Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 17 Feb 2014 08:43:59 +0100 Subject: [PATCH 58/68] Add a display output. --- src/software/globalSfM/SfMGlobalEngine.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/software/globalSfM/SfMGlobalEngine.cpp b/src/software/globalSfM/SfMGlobalEngine.cpp index 78faf9f5f5..2415a8f45e 100644 --- a/src/software/globalSfM/SfMGlobalEngine.cpp +++ b/src/software/globalSfM/SfMGlobalEngine.cpp @@ -271,6 +271,7 @@ bool GlobalReconstructionEngine::Process() //---------------------------- std::map map_globalR; + std::cout << "Compute " << map_cameraIndexTocameraNode.size() << "global rotations." << std::endl; { // Build relative information for only the largest considered Connected Component @@ -1445,7 +1446,7 @@ void GlobalReconstructionEngine::tripletRotationRejection( sg); { - std::cout << "\nTriplets filtering based on error on cycle \n"; + std::cout << "\nTriplets filtering based on error on cycles \n"; std::cout << "Before : " << vec_triplets.size() << " triplets \n" << "After : " << vec_triplets_validated.size() << std::endl; std::cout << "There is " << lemon::countConnectedComponents (sg) From 858b7b4eab2c6ef36336a1caff073b5c8d91363b Mon Sep 17 00:00:00 2001 From: pmoulon Date: Mon, 17 Feb 2014 08:45:38 +0100 Subject: [PATCH 59/68] Improve program stability in case of initial pair with very small relative angle. #92 --- src/software/SfM/SfMIncrementalEngine.cpp | 86 +++++++++++++---------- 1 file changed, 48 insertions(+), 38 deletions(-) diff --git a/src/software/SfM/SfMIncrementalEngine.cpp b/src/software/SfM/SfMIncrementalEngine.cpp index 317628218b..0d1ecb7964 100644 --- a/src/software/SfM/SfMIncrementalEngine.cpp +++ b/src/software/SfM/SfMIncrementalEngine.cpp @@ -95,9 +95,6 @@ bool IncrementalReconstructionEngine::Process() if(! MakeInitialPair3D(initialPairIndex)) return false; - _vec_added_order.push_back(initialPairIndex.first); - _vec_added_order.push_back(initialPairIndex.second); - BundleAdjustment(); // Adjust 3D point and camera parameters. size_t round = 0; @@ -475,30 +472,9 @@ bool IncrementalReconstructionEngine::MakeInitialPair3D(const std::pair Triangulate the common tracks //--> Triangulate the point - // Add information related to the View (I,J) to the reconstruction data - _reconstructorData.set_imagedId.insert(I); - _reconstructorData.set_imagedId.insert(J); - BrownPinholeCamera camI(intrinsicCamI.m_focal, intrinsicCamI.m_K(0,2), intrinsicCamI.m_K(1,2), Mat3::Identity(), Vec3::Zero()); BrownPinholeCamera camJ(intrinsicCamJ.m_focal, intrinsicCamJ.m_K(0,2), intrinsicCamJ.m_K(1,2), RJ, tJ); - _reconstructorData.map_Camera.insert(std::make_pair(I, camI)); - _reconstructorData.map_Camera.insert(std::make_pair(J, camJ)); - - // Add the images to the reconstructed intrinsic group - _map_ImagesIdPerIntrinsicGroup[_map_IntrinsicIdPerImageId[I]].push_back(I); - _map_ImagesIdPerIntrinsicGroup[_map_IntrinsicIdPerImageId[J]].push_back(J); - // Initialize the first intrinsics groups: - Vec6 & intrinsicI = _map_IntrinsicsPerGroup[_map_IntrinsicIdPerImageId[I]]; - intrinsicI<< camI._f, camI._ppx, camI._ppy, camI._k1, camI._k2, camI._k3; - Vec6 & intrinsicJ = _map_IntrinsicsPerGroup[_map_IntrinsicIdPerImageId[J]]; - intrinsicJ<< camJ._f, camJ._ppx, camJ._ppy, camJ._k1, camJ._k2, camJ._k3; - - _reconstructorData.map_ACThreshold.insert(std::make_pair(I, errorMax)); - _reconstructorData.map_ACThreshold.insert(std::make_pair(J, errorMax)); - - _set_remainingImageId.erase(I); - _set_remainingImageId.erase(J); - + std::vector vec_index; for (openMVG::tracks::STLMAPTracks::const_iterator iterT = map_tracksCommon.begin(); @@ -559,6 +535,35 @@ bool IncrementalReconstructionEngine::MakeInitialPair3D(const std::pairgetDoc(); } - return true; + return !_reconstructorData.map_3DPoints.empty(); } /// Functor to sort a vector of pair given the pair's second value @@ -640,7 +645,8 @@ struct sort_pair_second { bool IncrementalReconstructionEngine::FindImagesWithPossibleResection(std::vector & vec_possible_indexes) { vec_possible_indexes.clear(); - if (_set_remainingImageId.empty()) { + + if (_set_remainingImageId.empty() || _map_reconstructed.empty()) { return false; } @@ -664,18 +670,21 @@ bool IncrementalReconstructionEngine::FindImagesWithPossibleResection(std::vecto set_imageIndex.insert(imageIndex); TracksUtilsMap::GetTracksInImages(set_imageIndex, _map_tracks, map_tracksCommon); - std::set set_tracksIds; - TracksUtilsMap::GetTracksIdVector(map_tracksCommon, &set_tracksIds); + if (! map_tracksCommon.empty()) + { + std::set set_tracksIds; + TracksUtilsMap::GetTracksIdVector(map_tracksCommon, &set_tracksIds); - // Count the common possible putative point - // with the already 3D reconstructed trackId - std::vector vec_trackIdForResection; - set_intersection(set_tracksIds.begin(), set_tracksIds.end(), - _reconstructorData.set_trackId.begin(), - _reconstructorData.set_trackId.end(), - std::back_inserter(vec_trackIdForResection)); + // Count the common possible putative point + // with the already 3D reconstructed trackId + std::vector vec_trackIdForResection; + set_intersection(set_tracksIds.begin(), set_tracksIds.end(), + _reconstructorData.set_trackId.begin(), + _reconstructorData.set_trackId.end(), + std::back_inserter(vec_trackIdForResection)); - vec_putative.push_back( make_pair(imageIndex, vec_trackIdForResection.size())); + vec_putative.push_back( make_pair(imageIndex, vec_trackIdForResection.size())); + } } if (vec_putative.empty()) { @@ -1426,6 +1435,7 @@ void IncrementalReconstructionEngine::BundleAdjustment() } //-- Lock the first camera to better deal with scene orientation ambiguity + if (_vec_added_order.size()>0 && map_camIndexToNumber_extrinsic.size()>0) { // First camera is the first one that have been used problem.SetParameterBlockConstant( @@ -1445,7 +1455,7 @@ void IncrementalReconstructionEngine::BundleAdjustment() options.sparse_linear_algebra_library_type = ceres::CX_SPARSE; else { - // No sparse backend for Ceres. + // No sparse back end for Ceres. // Use dense solving options.linear_solver_type = ceres::DENSE_SCHUR; } From 7eee59e5c06ba5343f895d32b6879fe6268b5cad Mon Sep 17 00:00:00 2001 From: pmoulon Date: Fri, 21 Feb 2014 16:26:49 +0100 Subject: [PATCH 60/68] Update openMVG2PMVS. Add export in Bundle.rd.out format. #94 --- src/software/SfM/main_openMVG2PMVS.cpp | 78 ++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/src/software/SfM/main_openMVG2PMVS.cpp b/src/software/SfM/main_openMVG2PMVS.cpp index 4b5f94bea2..1877d9d730 100644 --- a/src/software/SfM/main_openMVG2PMVS.cpp +++ b/src/software/SfM/main_openMVG2PMVS.cpp @@ -113,6 +113,77 @@ bool exportToPMVSFormat( return bOk; } + +bool exportToBundlerFormat( + const Document & doc, + const std::string & sOutFile, //Output Bundle.rd.out file + const std::string & sOutListFile) //Output Bundler list.txt file +{ + std::ofstream os(sOutFile.c_str() ); + std::ofstream osList(sOutListFile.c_str() ); + if (os.is_open() && osList.is_open()) + { + os << "# Bundle file v0.3" << std::endl + << doc._map_camera.size() + << " " << doc._tracks.size() << std::endl; + + size_t count = 0; + for (std::map::const_iterator iter = doc._map_camera.begin(); + iter != doc._map_camera.end(); ++iter) + { + const PinholeCamera & PMat = iter->second; + + Mat3 D; + D.fill(0.0); + D .diagonal() = Vec3(1., -1., -1.); // mapping between our pinhole and Bundler convention + Mat3 R = D * PMat._R; + Vec3 t = D * PMat._t; + double focal = PMat._K(0,0); + double k1 = 0.0, k2 = 0.0; // distortion already removed + + os << focal << " " << k1 << " " << k2 << std::endl //f k1 k2 + << R(0,0) << " " << R(0, 1) << " " << R(0, 2) << std::endl //R[0] + << R(1,0) << " " << R(1, 1) << " " << R(1, 2) << std::endl //R[1] + << R(2,0) << " " << R(2, 1) << " " << R(2, 2) << std::endl //R[2] + << t(0) << " " << t(1) << " " << t(2) << std::endl; //t + + osList << doc._vec_imageNames[iter->first] << " 0 " << focal << std::endl; + } + + for (std::map< size_t, tracks::submapTrack >::const_iterator + iterTracks = doc._tracks.begin(); + iterTracks != doc._tracks.end(); + ++iterTracks) + { + const size_t trackId = iterTracks->first; + const tracks::submapTrack & map_track = iterTracks->second; + const size_t trackIndex = + std::distance::const_iterator>(doc._tracks.begin(), iterTracks); + + const float * ptr3D = & doc._vec_points[trackIndex*3]; + os << ptr3D[0] << " " << ptr3D[1] << " " << ptr3D[2] << std::endl; + os << "255 255 255" << std::endl; + os << map_track.size() << " "; + for (tracks::submapTrack::const_iterator iterTrack = map_track.begin(); + iterTrack != map_track.end(); + ++iterTrack) + { + const PinholeCamera & PMat = doc._map_camera.find(iterTrack->first)->second; + Vec2 pt = PMat.Project(Vec3(ptr3D[0], ptr3D[1], ptr3D[2])); + os << iterTrack->first << " " << iterTrack->second << " " << pt(0) << " " << pt(1) << " "; + } + os << std::endl; + } + os.close(); + osList.close(); + } + else + { + return false; + } + return true; +} + int main(int argc, char *argv[]) { CmdLine cmd; @@ -154,6 +225,13 @@ int main(int argc, char *argv[]) { resolution, CPU ); + exportToBundlerFormat(m_doc, + stlplus::folder_append_separator(sOutDir) + + stlplus::folder_append_separator("PMVS") + "bundle.rd.out", + stlplus::folder_append_separator(sOutDir) + + stlplus::folder_append_separator("PMVS") + "list.txt" + ); + return( EXIT_SUCCESS ); } From 0456c80afe2400952ed00fa9f3f632f879440501 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Fri, 21 Feb 2014 16:27:40 +0100 Subject: [PATCH 61/68] Change SIFTdescriptor from a float array to an unsigned char array. --- src/software/SfM/main_computeMatches.cpp | 2 +- src/software/globalSfM/main_computeMatchesEssential.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/SfM/main_computeMatches.cpp b/src/software/SfM/main_computeMatches.cpp index 6cf7cc617a..95f67c4a43 100644 --- a/src/software/SfM/main_computeMatches.cpp +++ b/src/software/SfM/main_computeMatches.cpp @@ -198,7 +198,7 @@ int main(int argc, char **argv) // - else save features and descriptors on disk //--------------------------------------- - typedef Descriptor DescriptorT; + typedef Descriptor DescriptorT; typedef SIOPointFeature FeatureT; typedef std::vector FeatsT; typedef vector DescsT; diff --git a/src/software/globalSfM/main_computeMatchesEssential.cpp b/src/software/globalSfM/main_computeMatchesEssential.cpp index 1c62053271..be359d8c65 100644 --- a/src/software/globalSfM/main_computeMatchesEssential.cpp +++ b/src/software/globalSfM/main_computeMatchesEssential.cpp @@ -160,7 +160,7 @@ int main(int argc, char **argv) // - else save features and descriptors on disk //--------------------------------------- - typedef Descriptor DescriptorT; + typedef Descriptor DescriptorT; typedef SIOPointFeature FeatureT; typedef std::vector FeatsT; typedef vector DescsT; From 246648a9318f3bf9501d9a94058de740446ac4af Mon Sep 17 00:00:00 2001 From: pmoulon Date: Fri, 21 Feb 2014 16:43:43 +0100 Subject: [PATCH 62/68] update file opening test @fabiencastan. --- src/software/SfM/main_openMVG2PMVS.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/software/SfM/main_openMVG2PMVS.cpp b/src/software/SfM/main_openMVG2PMVS.cpp index 1877d9d730..a9443dd66a 100644 --- a/src/software/SfM/main_openMVG2PMVS.cpp +++ b/src/software/SfM/main_openMVG2PMVS.cpp @@ -121,7 +121,11 @@ bool exportToBundlerFormat( { std::ofstream os(sOutFile.c_str() ); std::ofstream osList(sOutListFile.c_str() ); - if (os.is_open() && osList.is_open()) + if (! os.is_open() || ! osList.is_open()) + { + return false; + } + else { os << "# Bundle file v0.3" << std::endl << doc._map_camera.size() @@ -177,10 +181,6 @@ bool exportToBundlerFormat( os.close(); osList.close(); } - else - { - return false; - } return true; } From d7e10b9d18658dc8764d39c2c00bad30e6dce03d Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 2 Mar 2014 12:04:40 +0100 Subject: [PATCH 63/68] Update MVS doc. #49 --- docs/sphinx/rst/software/SfM/MVS.rst | 43 +++++++++++++++++++--- docs/sphinx/rst/software/SfM/globalSfM.rst | 2 +- 2 files changed, 38 insertions(+), 7 deletions(-) diff --git a/docs/sphinx/rst/software/SfM/MVS.rst b/docs/sphinx/rst/software/SfM/MVS.rst index 84396107bd..1b315b7c78 100644 --- a/docs/sphinx/rst/software/SfM/MVS.rst +++ b/docs/sphinx/rst/software/SfM/MVS.rst @@ -1,21 +1,52 @@ +************************************* Multiple View Stereovision -==================================== - -Multiple View Stereo-vision -***************************** +************************************* Once camera position and orientation have been computed, Multiple View Stereo-vision algorithms could be used -to compute a dense scene representation. +to compute a dense scene representation, such as: + +- dense point cloud, +- surface and texture. + + +Export to PMVS/CMVS +======================== +OpenMVG exports [PMVS]_ ready to use project (images, projection matrices and pmvs_options.txt files). -OpenMVG exports [PMVS]_ and [CMPMVS]_ ready to use project (images, projection matrices and pmvs_options.txt files). +Once a 3D calibration have been computed you can convert the SfM_Ouput files to a PMVS project. + +.. code-block:: c++ + + $ openMVG_main_openMVG2PMVS -i Dataset/outReconstruction/SfM_Output/ -o Dataset/outReconstruction/SfM_Output/ + $ pmvs Dataset/outReconstruction/SfM_Output/PMVS/ pmvs_options.txt .. figure:: resultOutput.png :align: center Figure: Multiple View Stereo-vision point cloud densification on the estimated scene using [PMVS]_. +In order to use CMVS for large scene openMVG2PMVS export also the scene in the Bundler output format. + +.. code-block:: c++ + + $ openMVG_main_openMVG2PMVS -i Dataset/outReconstruction/SfM_Output/ -o Dataset/outReconstruction/SfM_Output/ + $ cmvs Dataset/outReconstruction/SfM_Output/PMVS/ + $ genOption Dataset/outReconstruction/SfM_Output/PMVS/ + $ sh Dataset/outReconstruction/SfM_Output/PMVS/pmvs.sh + + +Export to CMPMVS +======================== + +OpenMVG exports [CMPMVS]_ ready to use project (images, projection matrices and ini configuration file). + +.. code-block:: c++ + + $ openMVG_main_openMVG2CMPMVS -i Dataset/outReconstruction/SfM_Output/ -o Dataset/outReconstruction/SfM_Output/ + + .. [PMVS] Accurate, dense, and robust multi-view stereopsis. Yasutaka Furukawa and Jean Ponce. IEEE Trans. on Pattern Analysis and Machine Intelligence, 32(8):1362-1376, 2010. diff --git a/docs/sphinx/rst/software/SfM/globalSfM.rst b/docs/sphinx/rst/software/SfM/globalSfM.rst index 3e36e9fc08..91a462c866 100644 --- a/docs/sphinx/rst/software/SfM/globalSfM.rst +++ b/docs/sphinx/rst/software/SfM/globalSfM.rst @@ -47,7 +47,7 @@ See intrinsic SfM entry. 2. Geometric features correspondences computation -------------------------------------------------- -As approximative or precise focal length is know we can use the essential matrix to filter putative matches. +An approximative or precise focal length is know we can use the essential matrix to filter putative matches. .. code-block:: c++ From 52ece2cb6d5bce1fd506a8827026873c0e120e91 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 2 Mar 2014 12:10:30 +0100 Subject: [PATCH 64/68] remove computeMatchesEssential since computeMatches can do the job. #73 --- src/software/globalSfM/CMakeLists.txt | 7 - .../main_computeMatchesEssential.cpp | 325 ------------------ 2 files changed, 332 deletions(-) delete mode 100644 src/software/globalSfM/main_computeMatchesEssential.cpp diff --git a/src/software/globalSfM/CMakeLists.txt b/src/software/globalSfM/CMakeLists.txt index 5b97563d6a..3e16df40f1 100644 --- a/src/software/globalSfM/CMakeLists.txt +++ b/src/software/globalSfM/CMakeLists.txt @@ -16,10 +16,3 @@ TARGET_LINK_LIBRARIES( ${LIBS} openMVG_linearProgramming openMVG_lInftyComputerVision) - -ADD_EXECUTABLE( - openMVG_main_computeMatchesEssential - main_computeMatchesEssential.cpp) -TARGET_LINK_LIBRARIES(openMVG_main_computeMatchesEssential - ${LIBS}) - diff --git a/src/software/globalSfM/main_computeMatchesEssential.cpp b/src/software/globalSfM/main_computeMatchesEssential.cpp deleted file mode 100644 index be359d8c65..0000000000 --- a/src/software/globalSfM/main_computeMatchesEssential.cpp +++ /dev/null @@ -1,325 +0,0 @@ - -// Copyright (c) 2012, 2013, 2014 Pierre MOULON. - -// 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 http://mozilla.org/MPL/2.0/. - - -#include "openMVG/image/image.hpp" -#include "openMVG/features/features.hpp" - -/// Generic Image Collection image matching -#include "openMVG/matching_image_collection/Matcher_AllInMemory.hpp" -#include "openMVG/matching_image_collection/GeometricFilter.hpp" -#include "openMVG/matching_image_collection/E_ACRobust.hpp" -#include "software/SfM/pairwiseAdjacencyDisplay.hpp" -#include "software/SfM/SfMIOHelper.hpp" -#include "openMVG/matching/matcher_brute_force.hpp" -#include "openMVG/matching/matcher_kdtree_flann.hpp" -#include "openMVG/matching/indMatch_utils.hpp" - -/// Feature detector and descriptor interface -#include "patented/sift/SIFT.hpp" - -#include "third_party/cmdLine/cmdLine.h" - -#include "third_party/stlplus3/filesystemSimplified/file_system.hpp" -#include "third_party/progress/progress.hpp" - -#include "software/globalSfM/indexedImageGraph.hpp" -#include "software/globalSfM/indexedImageGraphExport.hpp" - -#include -#include -#include -#include -#include - -using namespace openMVG; -using namespace openMVG::matching; -using namespace openMVG::robust; -using namespace std; - -bool testIntrinsicsEquality( - SfMIO::IntrinsicCameraInfo const &ci1, - SfMIO::IntrinsicCameraInfo const &ci2) -{ - return ci1.m_K == ci2.m_K; -} - -int main(int argc, char **argv) -{ - CmdLine cmd; - - std::string sImaDirectory; - std::string sOutDir = ""; - float fDistRatio = .6f; - bool bOctMinus1 = false; - float dPeakThreshold = 0.01f; - - cmd.add( make_option('i', sImaDirectory, "imadir") ); - cmd.add( make_option('o', sOutDir, "outdir") ); - cmd.add( make_option('r', fDistRatio, "distratio") ); - cmd.add( make_option('s', bOctMinus1, "octminus1") ); - cmd.add( make_option('p', dPeakThreshold, "peakThreshold") ); - - try { - if (argc == 1) throw std::string("Invalid command line parameter."); - cmd.process(argc, argv); - } catch(const std::string& s) { - std::cerr << "Usage: " << argv[0] << ' ' - << "[-i|--imadir path] " - << "[-o|--outdir path] " - << "[-r|--distratio 0.6] " - << "[-s|--octminus1 0 or 1] " - << "[-p|--peakThreshold 0.04 -> 0.01] " - << std::endl; - - std::cerr << s << std::endl; - return EXIT_FAILURE; - } - - std::cout << " You called : " < vec_camImageName; - std::vector vec_focalGroup; - if (!openMVG::SfMIO::loadImageList( vec_camImageName, - vec_focalGroup, - sListsFile) ) - { - std::cerr << "\nEmpty image list." << std::endl; - return false; - } - - std::vector::iterator iterF = - std::unique(vec_focalGroup.begin(), vec_focalGroup.end(), testIntrinsicsEquality); - vec_focalGroup.resize( std::distance(vec_focalGroup.begin(), iterF) ); - if (vec_focalGroup.size() == 1) - { - for (size_t i = 0; i < vec_camImageName.size(); ++i) - vec_camImageName[i].m_intrinsicId = 0; - } - else - { - std::cout << "There is more than one focal group in the lists.txt file." << std::endl - << "Only one focal group is supported for the global calibration chain" << std::endl; - return EXIT_FAILURE; - } - - std::vector vec_fileNames; - std::vector > vec_imagesSize; - for ( std::vector::const_iterator iter_camInfo = vec_camImageName.begin(); - iter_camInfo != vec_camImageName.end(); - iter_camInfo++ ) - { - vec_imagesSize.push_back( std::make_pair( vec_focalGroup[iter_camInfo->m_intrinsicId].m_w, - vec_focalGroup[iter_camInfo->m_intrinsicId].m_h ) ); - vec_fileNames.push_back( stlplus::create_filespec( sImaDirectory, iter_camInfo->m_sImageName) ); - } - - //--------------------------------------- - // b. Compute features and descriptors - // - extract sift features and descriptors - // - if keypoints already computed, re-load them - // - else save features and descriptors on disk - //--------------------------------------- - - typedef Descriptor DescriptorT; - typedef SIOPointFeature FeatureT; - typedef std::vector FeatsT; - typedef vector DescsT; - typedef KeypointSet KeypointSetT; - - { - std::cout << "\n\nEXTRACT FEATURES" << std::endl; - vec_imagesSize.resize(vec_fileNames.size()); - - Image imageRGB; - Image imageGray; - - C_Progress_display my_progress_bar( vec_fileNames.size() ); - for(size_t i=0; i < vec_fileNames.size(); ++i) { - KeypointSetT kpSet; - - std::string sFeat = stlplus::create_filespec(sOutDir, - stlplus::basename_part(vec_fileNames[i]), "feat"); - std::string sDesc = stlplus::create_filespec(sOutDir, - stlplus::basename_part(vec_fileNames[i]), "desc"); - - //Test if descriptors and features was already computed - if (stlplus::file_exists(sFeat) && stlplus::file_exists(sDesc)) { - - if (ReadImage(vec_fileNames[i].c_str(), &imageRGB)) { - vec_imagesSize[i] = make_pair(imageRGB.Width(), imageRGB.Height()); - } - else { - ReadImage(vec_fileNames[i].c_str(), &imageGray); - vec_imagesSize[i] = make_pair(imageGray.Width(), imageGray.Height()); - } - } - else { //Not already computed, so compute and save - - if (ReadImage(vec_fileNames[i].c_str(), &imageRGB)) { - Rgb2Gray(imageRGB, &imageGray); - } - else{ - ReadImage(vec_fileNames[i].c_str(), &imageGray); - } - - // Compute features and descriptors and export them to file - SIFTDetector(imageGray, - kpSet.features(), kpSet.descriptors(), - bOctMinus1, true, dPeakThreshold); - - kpSet.saveToBinFile(sFeat, sDesc); - vec_imagesSize[i] = make_pair(imageGray.Width(), imageRGB.Height()); - } - ++my_progress_bar; - } - } - - //--------------------------------------- - // c. Compute putatives descriptor matches - // - L2 descriptor matching - // - Keep correspondences only if NearestNeighbor ratio is ok - //--------------------------------------- - IndexedMatchPerPair map_PutativesMatches; - // Define the matcher and the used metric (Squared L2) - // ANN matcher could be defined as follow: - typedef flann::L2 MetricT; - typedef ArrayMatcher_Kdtree_Flann MatcherT; - // Brute force matcher is defined as following: - //typedef L2_Vectorized MetricT; - //typedef ArrayMatcherBruteForce MatcherT; - - // If the matches already exists, reload them - if (stlplus::file_exists(sOutDir + "/matches.putative.txt")) - { - PairedIndMatchImport(sOutDir + "/matches.putative.txt", map_PutativesMatches); - std::cout << std::endl << "PUTATIVE MATCHES -- PREVIOUS RESULTS LOADED" << std::endl; - } - else // Compute the putatives matches - { - Matcher_AllInMemory collectionMatcher(fDistRatio); - if (collectionMatcher.loadData(vec_fileNames, sOutDir)) - { - std::cout << std::endl << "PUTATIVE MATCHES" << std::endl; - collectionMatcher.Match(vec_fileNames, map_PutativesMatches); - //--------------------------------------- - //-- Export putative matches - //--------------------------------------- - std::ofstream file (std::string(sOutDir + "/matches.putative.txt").c_str()); - if (file.is_open()) - PairedIndMatchToStream(map_PutativesMatches, file); - file.close(); - } - } - //-- export putative matches Adjacency matrix - PairWiseMatchingToAdjacencyMatrixSVG(vec_fileNames.size(), - map_PutativesMatches, - stlplus::create_filespec(sOutDir, "PutativeAdjacencyMatrix", "svg")); - - - //--------------------------------------- - // d. Geometric filtering of putatives matches - // - AContrario Estimation of the Essential matrix - // - Use a upper bound for the plausible E matrix - // acontrario estimated threshold - //--------------------------------------- - IndexedMatchPerPair map_GeometricMatches_E; - ImageCollectionGeometricFilter collectionGeomFilter; - if (collectionGeomFilter.loadData(vec_fileNames, sOutDir)) - { - std::cout << std::endl << " - GEOMETRIC FILTERING - " << std::endl; - collectionGeomFilter.Filter( - GeometricFilter_EMatrix_AC(vec_focalGroup[0].m_K, 4.0), - map_PutativesMatches, - map_GeometricMatches_E, - vec_imagesSize); - - //-- Perform an additional check to remove pairs with poor overlap - std::vector vec_toRemove; - for (IndexedMatchPerPair::const_iterator iterMap = map_GeometricMatches_E.begin(); - iterMap != map_GeometricMatches_E.end(); ++iterMap) - { - size_t putativePhotometricCount = map_PutativesMatches.find(iterMap->first)->second.size(); - size_t putativeGeometricCount = iterMap->second.size(); - float ratio = putativeGeometricCount / (float)putativePhotometricCount; - if (putativeGeometricCount < 50 || ratio < .30) { - // the pair will be removed - vec_toRemove.push_back(iterMap->first); - } - } - //-- remove discarded pairs - for (std::vector::const_iterator iter = vec_toRemove.begin(); - iter != vec_toRemove.end(); - ++iter) - { - map_GeometricMatches_E.erase(*iter); - } - - //--------------------------------------- - //-- Export geometric filtered matches - //--------------------------------------- - std::ofstream file (string(sOutDir + "/matches.e.txt").c_str()); - if (file.is_open()) - PairedIndMatchToStream(map_GeometricMatches_E, file); - file.close(); - - //-- export Adjacency matrix - std::cout << "\n Export Adjacency Matrix of the pairwise's Epipolar matches" - << std::endl; - PairWiseMatchingToAdjacencyMatrixSVG(vec_fileNames.size(), - map_GeometricMatches_E, - stlplus::create_filespec(sOutDir, "EpipolarAdjacencyMatrix", "svg")); - - //-- Export geometric Essential graph - //--------------------------------------- - typedef lemon::ListGraph Graph; - imageGraph::indexedImageGraph putativeGraph(map_GeometricMatches_E, vec_fileNames); - - // Save the graph before cleaning: - imageGraph::exportToGraphvizData( - stlplus::create_filespec(sOutDir, "essentialGraph"), - putativeGraph.g); - } - return EXIT_SUCCESS; -} - - From 379b754f591de646beb64005a5efaf46005e5e20 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 2 Mar 2014 12:13:50 +0100 Subject: [PATCH 65/68] Make USE_OPENMP preprocessor usage consistent --- src/openMVG/matching/kvld/kvld.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/openMVG/matching/kvld/kvld.cpp b/src/openMVG/matching/kvld/kvld.cpp index ebf72d7475..56d7bf7032 100644 --- a/src/openMVG/matching/kvld/kvld.cpp +++ b/src/openMVG/matching/kvld/kvld.cpp @@ -34,7 +34,7 @@ ImageScale::ImageScale( const Image< float >& I, double r ) GradAndNorm( I, angles[ 0 ], magnitudes[ 0 ] ); ratios[ 0 ] = 1; -#ifdef _OPENMP +#ifdef USE_OPENMP #pragma omp parallel for #endif for( int k = 1; k < number; k++ ) @@ -63,7 +63,7 @@ void ImageScale::GradAndNorm( const Image< float >& I, Image< float >& angle, Im m = Image< float >( I.Width(), I.Height() ); angle.fill( 0 ); m.fill( 0 ); -#ifdef _OPENMP +#ifdef USE_OPENMP #pragma omp parallel for #endif for( int x = 1; x < I.Width() - 1; x++ ) From 49b4c0f5904f5448d20a8b0f5fb815d8836d2a94 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 2 Mar 2014 17:52:37 +0100 Subject: [PATCH 66/68] Update MVS doc. #49 --- docs/sphinx/rst/software/SfM/MVS.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/sphinx/rst/software/SfM/MVS.rst b/docs/sphinx/rst/software/SfM/MVS.rst index 1b315b7c78..c81513cc8e 100644 --- a/docs/sphinx/rst/software/SfM/MVS.rst +++ b/docs/sphinx/rst/software/SfM/MVS.rst @@ -32,7 +32,8 @@ In order to use CMVS for large scene openMVG2PMVS export also the scene in the B .. code-block:: c++ $ openMVG_main_openMVG2PMVS -i Dataset/outReconstruction/SfM_Output/ -o Dataset/outReconstruction/SfM_Output/ - $ cmvs Dataset/outReconstruction/SfM_Output/PMVS/ + $ cmvs Dataset/outReconstruction/SfM_Output/PMVS/ [MaxImageCountByCluster=100] + $ cmvs Dataset/outReconstruction/SfM_Output/PMVS/ 30 $ genOption Dataset/outReconstruction/SfM_Output/PMVS/ $ sh Dataset/outReconstruction/SfM_Output/PMVS/pmvs.sh From 1acd9137bb31923a12df7d26a89ec99d1ea09b5d Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 2 Mar 2014 17:52:56 +0100 Subject: [PATCH 67/68] Improve text output. --- src/software/globalSfM/SfMGlobalEngine.cpp | 39 +++++++++++----------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/src/software/globalSfM/SfMGlobalEngine.cpp b/src/software/globalSfM/SfMGlobalEngine.cpp index eada737457..ff932dee13 100644 --- a/src/software/globalSfM/SfMGlobalEngine.cpp +++ b/src/software/globalSfM/SfMGlobalEngine.cpp @@ -271,7 +271,7 @@ bool GlobalReconstructionEngine::Process() //---------------------------- std::map map_globalR; - std::cout << "Compute " << map_cameraIndexTocameraNode.size() << "global rotations." << std::endl; + std::cout << "Compute " << map_cameraIndexTocameraNode.size() << " global rotations." << std::endl; { // Build relative information for only the largest considered Connected Component @@ -682,11 +682,12 @@ bool GlobalReconstructionEngine::Process() //-- Export data to openMVG format: { + std::cout << "\n Export calibration data and image data." << std::endl; const std::string & sOutDirectory = stlplus::folder_append_separator(_sOutDirectory) + "SfM_Output"; //Export directory const std::vector & vec_fileNames = _vec_fileNames; // vector of image filenames const std::string & sImagePath = _sImagePath; // The images path const openMVG::tracks::STLMAPTracks & map_reconstructed = map_tracksSelected; // Tracks (Visibility) - bool bExportImage = false; + bool bExportImage = true; bool bOk = true; if (!stlplus::is_folder(sOutDirectory)) @@ -743,20 +744,20 @@ bool GlobalReconstructionEngine::Process() // Cloud std::ofstream f( stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "clouds", - "calib", "ply").c_str()); - std::ofstream f_visibility( - stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "clouds", + "calib", "ply").c_str()); + std::ofstream f_visibility( + stlplus::create_filespec(stlplus::folder_append_separator(sOutDirectory) + "clouds", "visibility", "txt").c_str()); if (!f.is_open()) { std::cerr << "cannot save cloud" << std::endl; return false; - } - - if (!f_visibility.is_open()) { - std::cerr << "cannot save cloud desc" << std::endl; - return false; - } + } + + if (!f_visibility.is_open()) { + std::cerr << "cannot save cloud desc" << std::endl; + return false; + } f << "ply\nformat ascii 1.0\ncomment generated by the Global OpenMVG Calibration Engine" << "\n"; f << "element vertex " << vec_allScenes.size() << "\n"; @@ -775,7 +776,7 @@ bool GlobalReconstructionEngine::Process() const Vec3 & pos = vec_allScenes[i]; f << pos.transpose() << " " << "255 255 255" << " " << 3.14; - + std::ostringstream s_visibility; std::set< size_t > set_imageIndex; for( tracks::submapTrack::const_iterator iterTrack = track.begin(); @@ -791,20 +792,20 @@ bool GlobalReconstructionEngine::Process() double z = Depth(cam._R, cam._t, pos); znear[map_cameratoIndex[imageId]] = std::min(znear[map_cameratoIndex[imageId]], z ); zfar[map_cameratoIndex[imageId]] = std::max(zfar[map_cameratoIndex[imageId]], z ); - } + } s_visibility << iterTrack->first << " " << iterTrack->second << " "; - } + } //export images indexes f << " " << set_imageIndex.size() << " "; copy(set_imageIndex.begin(), set_imageIndex.end(), std::ostream_iterator(f, " ")); - f << std::endl; - - f_visibility << pos.transpose() << " " << set_imageIndex.size() << " "; + f << std::endl; + + f_visibility << pos.transpose() << " " << set_imageIndex.size() << " "; f_visibility << s_visibility.str() << std::endl; } - f.close(); + f.close(); f_visibility.close(); // Views @@ -911,7 +912,7 @@ bool GlobalReconstructionEngine::ReadInputData() for (size_t i = 0; i < _vec_camImageNames.size(); ++i) { - _vec_fileNames.push_back( stlplus::create_filespec( _sImagePath, _vec_camImageNames[i].m_sImageName) ); + _vec_fileNames.push_back(_vec_camImageNames[i].m_sImageName); } } } From d3fb39ad942a936f6534c09d36cf8d2b1fd1cd32 Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 2 Mar 2014 18:48:08 +0100 Subject: [PATCH 68/68] Fix doc merging from LInfinityCV --- docs/sphinx/rst/software/SfM/SfM.rst | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/docs/sphinx/rst/software/SfM/SfM.rst b/docs/sphinx/rst/software/SfM/SfM.rst index d63953b176..4630cfcde2 100644 --- a/docs/sphinx/rst/software/SfM/SfM.rst +++ b/docs/sphinx/rst/software/SfM/SfM.rst @@ -6,6 +6,7 @@ Structure from Motion computes an external camera pose per image (the motion) an Inputs are images and internal camera calibration information (intrinsic parameters). Feature points are detected in each image (e.g., SIFT) and matched between image pairs and then the SfM pipeline compute the scene and camera motion. There are three main approaches to solve the SfM problem: + - the incremental/sequential pipeline, - the hierarchical pipeline, - the global one. @@ -73,22 +74,9 @@ Using a 3 directories based data organisation structure is suggested: * **matches** -<<<<<<< HEAD - - If you want refine intrinsics (focal, principal point and radial distortion) for each focal group - .. code-block:: c++ - - $ openMVG_main_IncrementalSfM -i /home/pierre/Pictures/Dataset/images/ -m /home/pierre/Pictures/Dataset/matches/ -o /home/pierre/Pictures/Dataset/outReconstruction/ - - - If you want only refine the focal (to use with image were the distortion have been already removed) - .. code-block:: c++ - - - $ openMVG_main_IncrementalSfM -i /home/pierre/Pictures/Dataset/images/ -m /home/pierre/Pictures/Dataset/matches/ -o /home/pierre/Pictures/Dataset/outReconstruction/ -d 0 -======= * the image information (lists.txt), images points and matches information will be saved here * **outReconstruction** ->>>>>>> LInfinityCV * directory where result and log of the 3D reconstruction will be exported