From 28409bbb20b9ef323f8713d2e0782d075eef2884 Mon Sep 17 00:00:00 2001 From: Stephen Vincent Mather Date: Sun, 21 Dec 2014 13:14:51 -0500 Subject: [PATCH] add Spotscale orthophoto/textured mesh additions --- install.sh | 138 ++- odm_extract_utm/CMakeLists.txt | 18 + odm_extract_utm/src/Logger.cpp | 31 + odm_extract_utm/src/Logger.hpp | 68 ++ odm_extract_utm/src/UtmExtractor.cpp | 383 +++++++ odm_extract_utm/src/UtmExtractor.hpp | 96 ++ odm_extract_utm/src/main.cpp | 9 + odm_georef/CMakeLists.txt | 28 + odm_georef/src/FindTransform.cpp | 149 +++ odm_georef/src/FindTransform.hpp | 165 +++ odm_georef/src/Georef.cpp | 530 ++++++++++ odm_georef/src/Georef.cpp~ | 530 ++++++++++ odm_georef/src/Georef.hpp | 150 +++ odm_georef/src/Logger.cpp | 31 + odm_georef/src/Logger.hpp | 68 ++ odm_georef/src/main.cpp | 8 + odm_georef/src/modifiedPclFunctions.cpp | 336 ++++++ odm_georef/src/modifiedPclFunctions.hpp | 20 + odm_meshing/CMakeLists.txt | 26 + odm_meshing/src/Logger.cpp | 31 + odm_meshing/src/Logger.hpp | 68 ++ odm_meshing/src/OdmMeshing.cpp | 361 +++++++ odm_meshing/src/OdmMeshing.cpp~ | 361 +++++++ odm_meshing/src/OdmMeshing.hpp | 117 ++ odm_meshing/src/main.cpp | 20 + odm_orthophoto/CMakeLists.txt | 33 + odm_orthophoto/src/Logger.cpp | 31 + odm_orthophoto/src/Logger.hpp | 68 ++ odm_orthophoto/src/OdmOrthoPhoto.cpp | 646 +++++++++++ odm_orthophoto/src/OdmOrthoPhoto.hpp | 140 +++ odm_orthophoto/src/main.cpp | 8 + odm_texturing/CMakeLists.txt | 34 + odm_texturing/src/Logger.cpp | 31 + odm_texturing/src/Logger.hpp | 68 ++ odm_texturing/src/OdmTexturing.cpp | 1116 ++++++++++++++++++++ odm_texturing/src/OdmTexturing.cpp~ | 1116 ++++++++++++++++++++ odm_texturing/src/OdmTexturing.hpp | 211 ++++ odm_texturing/src/main.cpp | 15 + odm_texturing/src/modifiedPclFunctions.cpp | 336 ++++++ odm_texturing/src/modifiedPclFunctions.hpp | 20 + run.pl | 168 ++- 41 files changed, 7760 insertions(+), 23 deletions(-) create mode 100644 odm_extract_utm/CMakeLists.txt create mode 100644 odm_extract_utm/src/Logger.cpp create mode 100644 odm_extract_utm/src/Logger.hpp create mode 100644 odm_extract_utm/src/UtmExtractor.cpp create mode 100644 odm_extract_utm/src/UtmExtractor.hpp create mode 100644 odm_extract_utm/src/main.cpp create mode 100644 odm_georef/CMakeLists.txt create mode 100644 odm_georef/src/FindTransform.cpp create mode 100644 odm_georef/src/FindTransform.hpp create mode 100644 odm_georef/src/Georef.cpp create mode 100644 odm_georef/src/Georef.cpp~ create mode 100644 odm_georef/src/Georef.hpp create mode 100644 odm_georef/src/Logger.cpp create mode 100644 odm_georef/src/Logger.hpp create mode 100644 odm_georef/src/main.cpp create mode 100644 odm_georef/src/modifiedPclFunctions.cpp create mode 100644 odm_georef/src/modifiedPclFunctions.hpp create mode 100644 odm_meshing/CMakeLists.txt create mode 100644 odm_meshing/src/Logger.cpp create mode 100644 odm_meshing/src/Logger.hpp create mode 100644 odm_meshing/src/OdmMeshing.cpp create mode 100644 odm_meshing/src/OdmMeshing.cpp~ create mode 100644 odm_meshing/src/OdmMeshing.hpp create mode 100644 odm_meshing/src/main.cpp create mode 100644 odm_orthophoto/CMakeLists.txt create mode 100644 odm_orthophoto/src/Logger.cpp create mode 100644 odm_orthophoto/src/Logger.hpp create mode 100644 odm_orthophoto/src/OdmOrthoPhoto.cpp create mode 100644 odm_orthophoto/src/OdmOrthoPhoto.hpp create mode 100644 odm_orthophoto/src/main.cpp create mode 100644 odm_texturing/CMakeLists.txt create mode 100644 odm_texturing/src/Logger.cpp create mode 100644 odm_texturing/src/Logger.hpp create mode 100644 odm_texturing/src/OdmTexturing.cpp create mode 100644 odm_texturing/src/OdmTexturing.cpp~ create mode 100644 odm_texturing/src/OdmTexturing.hpp create mode 100644 odm_texturing/src/main.cpp create mode 100644 odm_texturing/src/modifiedPclFunctions.cpp create mode 100644 odm_texturing/src/modifiedPclFunctions.hpp diff --git a/install.sh b/install.sh index 45ffc7040..2eb7171de 100755 --- a/install.sh +++ b/install.sh @@ -38,8 +38,15 @@ echo " - script started - `date`" VLFEAT_PATH="$TOOLS_SRC_PATH/vlfeat" PARALLEL_PATH="$TOOLS_SRC_PATH/parallel" PSR_PATH="$TOOLS_SRC_PATH/PoissonRecon" - GRACLUS_PATH="$TOOLS_SRC_PATH/graclus" - + GRACLUS_PATH="$TOOLS_SRC_PATH/graclus" + + PCL_PATH="$TOOLS_SRC_PATH/pcl" + ODM_MESHING_PATH="$TOOLS_SRC_PATH/odm_meshing" + ODM_TEXTURING_PATH="$TOOLS_SRC_PATH/odm_texturing" + ODM_ORTHOPHOTO_PATH="$TOOLS_SRC_PATH/odm_orthophoto" + ODM_EXTRACT_UTM_PATH="$TOOLS_SRC_PATH/odm_extract_utm" + ODM_GEOREF_PATH="$TOOLS_SRC_PATH/odm_georef" + ## executables EXTRACT_FOCAL="$TOOLS_BIN_PATH/extract_focal.pl" MATCHKEYS="$TOOLS_BIN_PATH/KeyMatch" @@ -54,6 +61,12 @@ echo " - script started - `date`" PSR="$TOOLS_BIN_PATH/PoissonRecon" VLSIFT_TO_LOWESIFT="$TOOLS_BIN_PATH/convert_vlsift_to_lowesift.pl" + ODM_MESHING="$TOOLS_BIN_PATH/odm_meshing" + ODM_TEXTURING="$TOOLS_BIN_PATH/odm_texturing" + ODM_ORTHOPHOTO="$TOOLS_BIN_PATH/odm_orthophoto" + ODM_EXTRACT_UTM="$TOOLS_BIN_PATH/odm_extract_utm" + ODM_GEOREF="$TOOLS_BIN_PATH/odm_georef" + ## get sys vars ARCH=`uname -m` CORES=`grep -c processor /proc/cpuinfo` @@ -75,6 +88,13 @@ mkdir -p "$TOOLS_LIB_PATH" mkdir -p "$TOOLS_SRC_PATH" mkdir -p "$TOOLS_LOG_PATH" +## Copy meshing and texturing to src folder +cp -rf "odm_meshing" "$TOOLS_SRC_PATH/" +cp -rf "odm_texturing" "$TOOLS_SRC_PATH/" +cp -rf "odm_orthophoto" "$TOOLS_SRC_PATH/" +cp -rf "odm_extract_utm" "$TOOLS_SRC_PATH/" +cp -rf "odm_georef" "$TOOLS_SRC_PATH/" + ## output sys info echo "System info:" > "$TOOLS_LOG_PATH/sysinfo.txt" uname -a > "$TOOLS_LOG_PATH/sysinfo.txt" @@ -91,10 +111,10 @@ sudo apt-get install --assume-yes --install-recommends \ build-essential cmake g++ gcc gFortran perl git \ curl wget \ unzip \ - imagemagick jhead \ - libjpeg-dev libboost-dev libgsl0-dev libx11-dev libxext-dev liblapack-dev \ + imagemagick jhead proj-bin libproj-dev\ + libjpeg-dev libboost1.48-all-dev libgsl0-dev libx11-dev libxext-dev liblapack-dev \ + libeigen3-dev libflann-dev libvtk5-dev libqhull-dev libusb-1.0-0-dev\ libzip-dev \ - libswitch-perl \ libcv-dev libcvaux-dev \ > "$TOOLS_LOG_PATH/apt-get_install.log" 2>&1 @@ -129,6 +149,7 @@ PoissonRecon.zip http://www.cs.jhu.edu/~misha/Code/PoissonRecon/Version2/Poisson vlfeat.tar.gz http://www.vlfeat.org/download/vlfeat-0.9.13-bin.tar.gz cmvs.tar.gz http://www.di.ens.fr/cmvs/cmvs-fix2.tar.gz graclus.tar.gz http://smathermather.github.io/BundlerTools/patched_files/src/graclus/graclus1.2.tar.gz +pcl.tar.gz https://github.com/PointCloudLibrary/pcl/archive/pcl-1.7.2.tar.gz EOF echo " < done - `date`" @@ -149,13 +170,15 @@ done wait -mv -f graclus1.2 "$GRACLUS_PATH" +mv -f graclus1.2 "$GRACLUS_PATH" mv -f clapack-3.2.1-CMAKE "$CLAPACK_PATH" mv -f vlfeat-0.9.13 "$VLFEAT_PATH" mv -f bundler-v0.4-source "$BUNDLER_PATH" mv -f parallel-20141022 "$PARALLEL_PATH" mv -f PoissonRecon "$PSR_PATH" mv -f cmvs "$CMVS_PATH" +mv -f pcl-pcl-1.7.2 "$PCL_PATH" + echo " < done - `date`" @@ -180,7 +203,6 @@ echo sudo chown -R `id -u`:`id -g` * #sudo chmod -R 777 * - echo " > graclus" cd "$GRACLUS_PATH" @@ -325,6 +347,108 @@ echo " > bundler" echo " < done - `date`" echo +echo " > pcl " + #cd "$PCL_PATH" + + #Install pcl dependencies using the default package manager. + #sudo apt-get install libeigen3-dev libflann-dev libvtk5-dev libqhull-dev + + #install the required boost version. + #sudo apt-get install libboost1.48-all-dev + + mkdir -p "pcl" + mkdir -p "$TOOLS_LIB_PATH/pcl" + mkdir -p "$PCL_PATH/pcl_tmp" + mkdir -p "$PCL_PATH/pcl_build" + + #mv -f "pcl-pcl-1.7.2" "$PCL_PATH/pcl_tmp" + + cd "$PCL_PATH/pcl_build" + + echo " - configuring pcl" + + cmake .. -DCMAKE_INSTALL_PREFIX="$TOOLS_LIB_PATH/pcl" -DCMAKE_BUILD_TYPE=Release -DPCL_VERBOSITY_LEVEL=Error -DBUILD_features=OFF -DBUILD_filters=OFF -DBUILD_geometry=OFF -DBUILD_keypoints=OFF -DBUILD_outofcore=OFF -DBUILD_people=OFF -DBUILD_recognition=OFF -DBUILD_registration=OFF -DBUILD_sample_consensus=OFF -DBUILD_segmentation=OFF -DBUILD_features=OFF -DBUILD_surface_on_nurbs=OFF -DBUILD_tools=OFF -DBUILD_tracking=OFF -DBUILD_visualization=OFF -DWITH_QT=OFF -DBUILD_OPENNI=OFF -DBUILD_OPENNI2=OFF -DWITH_OPENNI=OFF -DWITH_OPENNI2=OFF -DWITH_FZAPI=OFF -DWITH_LIBUSB=OFF -DWITH_PCAP=OFF -DWITH_PXCAPI=OFF > "$TOOLS_LOG_PATH/pcl_1_build.log" 2>&1 + + echo " - building and installing pcl" + make install > "$TOOLS_LOG_PATH/pcl_2_build.log" 2>&1 + +echo " < done - `date`" +echo + +echo " > meshing " + cd "$ODM_MESHING_PATH" + + echo " - configuring odm_meshing" + cmake . -DPCL_DIR="$TOOLS_LIB_PATH/pcl" > "$TOOLS_LOG_PATH/odm_meshing_1_build.log" 2>&1 + + echo " - building odm_meshing" + make > "$TOOLS_LOG_PATH/odm_meshing_2_build.log" 2>&1 + + # copy output program to the binaries folder. + cp -f "odm_meshing" "$TOOLS_BIN_PATH/odm_meshing" + +echo " < done - `date`" +echo + +echo " > texturing " + cd "$ODM_TEXTURING_PATH" + + echo " - configuring odm_texturing" + cmake . -DPCL_DIR="$TOOLS_LIB_PATH/pcl" > "$TOOLS_LOG_PATH/odm_texturing_1_build.log" 2>&1 + + echo " - building odm_texturing" + make > "$TOOLS_LOG_PATH/odm_texturing_2_build.log" 2>&1 + + # copy output program to the binaries folder. + cp -f "odm_texturing" "$TOOLS_BIN_PATH/odm_texturing" + +echo " < done - `date`" +echo + +echo " > extract_utm " + cd "$ODM_EXTRACT_UTM_PATH" + + echo " - configuring odm_extract_utm" + cmake . > "$TOOLS_LOG_PATH/odm_extract_utm_1_build.log" 2>&1 + + echo " - building odm_extract_utm" + make > "$TOOLS_LOG_PATH/odm_extract_utm_2_build.log" 2>&1 + + # copy output program to the binaries folder. + cp -f "odm_extract_utm" "$TOOLS_BIN_PATH/odm_extract_utm" + +echo " < done - `date`" +echo + +echo " > georef " + cd "$ODM_GEOREF_PATH" + + echo " - configuring odm_georef" + cmake . -DPCL_DIR="$TOOLS_LIB_PATH/pcl" > "$TOOLS_LOG_PATH/odm_georef_1_build.log" 2>&1 + + echo " - building odm_georef" + make > "$TOOLS_LOG_PATH/odm_georef_2_build.log" 2>&1 + + # copy output program to the binaries folder. + cp -f "odm_georef" "$TOOLS_BIN_PATH/odm_georef" + +echo " < done - `date`" +echo + +echo " > orthophoto " + cd "$ODM_ORTHOPHOTO_PATH" + + echo " - configuring odm_orthophoto" + cmake . -DPCL_DIR="$TOOLS_LIB_PATH/pcl" > "$TOOLS_LOG_PATH/odm_orthophoto_1_build.log" 2>&1 + + echo " - building odm_orthophoto" + make > "$TOOLS_LOG_PATH/odm_orthophoto_2_build.log" 2>&1 + + # copy output program to the binaries folder. + cp -f "odm_orthophoto" "$TOOLS_BIN_PATH/odm_orthophoto" + +echo " < done - `date`" +echo cd "$TOOLS_PATH" diff --git a/odm_extract_utm/CMakeLists.txt b/odm_extract_utm/CMakeLists.txt new file mode 100644 index 000000000..42cbb0490 --- /dev/null +++ b/odm_extract_utm/CMakeLists.txt @@ -0,0 +1,18 @@ +project(odm_extract_utm) +cmake_minimum_required(VERSION 2.8) + +set(PROJ4_INCLUDE_DIR "/usr/include/" CACHE "PROJ4_INCLUDE_DIR" "Path to the proj4 inlcude directory") +set(PROJ4_LIBRARY "/usr/lib/libproj.so" CACHE "PROJ4_LIBRARY" "Path to the proj4 library directory") + +# Add compiler options. +add_definitions(-Wall -Wextra) + +# Add source directory +aux_source_directory("./src" SRC_LIST) + +# Add exectuteable +add_executable(${PROJECT_NAME} ${SRC_LIST}) + +# Link +target_link_libraries(${PROJECT_NAME} ${PROJ4_LIBRARY}) + diff --git a/odm_extract_utm/src/Logger.cpp b/odm_extract_utm/src/Logger.cpp new file mode 100644 index 000000000..a6c81a8b5 --- /dev/null +++ b/odm_extract_utm/src/Logger.cpp @@ -0,0 +1,31 @@ +#include "Logger.hpp" + + +Logger::Logger(bool isPrintingInCout) : isPrintingInCout_(isPrintingInCout) +{ + +} + +Logger::~Logger() +{ + +} + +void Logger::printToFile(std::string filePath) +{ + std::ofstream file(filePath.c_str(), std::ios::binary); + file << logStream_.str(); + file.close(); +} + +bool Logger::isPrintingInCout() const +{ + return isPrintingInCout_; +} + +void Logger::setIsPrintingInCout(bool isPrintingInCout) +{ + isPrintingInCout_ = isPrintingInCout; +} + + diff --git a/odm_extract_utm/src/Logger.hpp b/odm_extract_utm/src/Logger.hpp new file mode 100644 index 000000000..31c5538cb --- /dev/null +++ b/odm_extract_utm/src/Logger.hpp @@ -0,0 +1,68 @@ +#pragma once + +// STL +#include +#include +#include +#include + +/*! + * \brief The Logger class is used to store program messages in a log file. + * \details By using the << operator while printInCout is set, the class writes both to + * cout and to file, if the flag is not set, output is written to file only. + */ +class Logger +{ +public: + /*! + * \brief Logger Contains functionality for printing and displaying log information. + * \param printInCout Flag toggling if operator << also writes to cout. + */ + Logger(bool isPrintingInCout = true); + + /*! + * \brief Destructor. + */ + ~Logger(); + + /*! + * \brief print Prints the contents of the log to file. + * \param filePath Path specifying where to write the log. + */ + void printToFile(std::string filePath); + + /*! + * \brief isPrintingInCout Check if console printing flag is set. + * \return Console printing flag. + */ + bool isPrintingInCout() const; + + /*! + * \brief setIsPrintingInCout Set console printing flag. + * \param isPrintingInCout Value, if true, messages added to the log are also printed in cout. + */ + void setIsPrintingInCout(bool isPrintingInCout); + + /*! + * Operator for printing messages to log and in the standard output stream if desired. + */ + template + friend Logger& operator<< (Logger &log, T t) + { + // If console printing is enabled. + if (log.isPrintingInCout_) + { + std::cout << t; + std::cout.flush(); + } + // Write to log. + log.logStream_ << t; + + return log; + } + +private: + bool isPrintingInCout_; /*!< If flag is set, log is printed in cout and written to the log. */ + + std::stringstream logStream_; /*!< Stream for storing the log. */ +}; diff --git a/odm_extract_utm/src/UtmExtractor.cpp b/odm_extract_utm/src/UtmExtractor.cpp new file mode 100644 index 000000000..bdd6b1cb2 --- /dev/null +++ b/odm_extract_utm/src/UtmExtractor.cpp @@ -0,0 +1,383 @@ +// STL +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Proj4 +#include + +// This +#include "UtmExtractor.hpp" + +UtmExtractor::UtmExtractor() : log_(false) +{ + logFile_ = "odm_extracting_utm_log.txt"; +} + +UtmExtractor::~UtmExtractor() +{ +} + + + +int UtmExtractor::run(int argc, char **argv) +{ + if (argc <= 1) + { + printHelp(); + return EXIT_SUCCESS; + } + + try + { + parseArguments(argc, argv); + extractUtm(); + } + catch (const UtmExtractorException& e) + { + log_.setIsPrintingInCout(true); + log_ << e.what() << "\n"; + log_.printToFile(logFile_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (const std::exception& e) + { + log_.setIsPrintingInCout(true); + log_ << "Error in OdmExtractUtm:\n"; + log_ << e.what() << "\n"; + log_.printToFile(logFile_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (...) + { + log_.setIsPrintingInCout(true); + log_ << "Unknown error in OdmExtractUtm:\n"; + log_.printToFile(logFile_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + + log_.printToFile(logFile_); + return EXIT_SUCCESS; +} + +void UtmExtractor::parseArguments(int argc, char **argv) +{ + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + // The argument to be parsed + std::string argument = std::string(argv[argIndex]); + if (argument == "-help") + { + printHelp(); + } + else if (argument == "-verbose") + { + log_.setIsPrintingInCout(true); + } + else if (argument == "-imageListFile") + { + ++argIndex; + if (argIndex >= argc) + { + throw UtmExtractorException("Missing argument for '" + argument + "'."); + } + imageListFileName_ = std::string(argv[argIndex]); + std::ifstream testFile(imageListFileName_.c_str(), std::ios_base::binary); + if (!testFile.is_open()) + { + throw UtmExtractorException("Argument '" + argument + "' has a bad value (file not accessible)."); + } + log_ << "imageListFile was set to: " << imageListFileName_ << "\n"; + } + else if (argument == "-imagesPath") + { + ++argIndex; + if (argIndex >= argc) + { + throw UtmExtractorException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> imagesPath_; + if (ss.bad()) + { + throw UtmExtractorException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "imagesPath was set to: " << imagesPath_ << "\n"; + } + else if (argument == "-outputCoordFile") + { + ++argIndex; + if (argIndex >= argc) + { + throw UtmExtractorException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> outputCoordFileName_; + if (ss.bad()) + { + throw UtmExtractorException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "outputCoordFile was set to: " << outputCoordFileName_ << "\n"; + } + else if (argument == "-logFile") + { + ++argIndex; + if (argIndex >= argc) + { + throw UtmExtractorException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> logFile_; + if (ss.bad()) + { + throw UtmExtractorException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "logFile_ was set to: " << logFile_ << "\n"; + } + else + { + printHelp(); + throw UtmExtractorException("Unrecognised argument '" + argument + "'."); + } + } + +} + +void UtmExtractor::extractUtm() +{ + // Open file listing all used camera images + std::ifstream imageListStream(imageListFileName_.c_str()); + if (!imageListStream.good()) { + throw UtmExtractorException("Failed to open " + imageListFileName_ + " for reading."); + } + + // Traverse images + int utmZone = 99; // for auto-select + char hemisphere; + std::string imageFilename; + std::vector coords; + while (getline(imageListStream, imageFilename)) { + // Run jhead on image to extract EXIF data to temporary file + std::string commandLine = "jhead -v " + imagesPath_ + "/" + imageFilename + " > extract_utm_output.txt"; + system(commandLine.c_str()); + + // Read temporary EXIF data file + std::ifstream jheadDataStream; + jheadDataStream.open("extract_utm_output.txt"); + if (!jheadDataStream.good()) { + throw UtmExtractorException("Failed to open temporary jhead data file extract_utm_output.txt"); + } + + // Delete temporary file + remove("extract_utm_output.txt"); + + // Parse jhead output + double lon, lat, alt; + if (!parsePosition(jheadDataStream, lon, lat, alt)) { + throw UtmExtractorException("Failed parsing GPS position."); + jheadDataStream.close(); + } + jheadDataStream.close(); + + // Convert to UTM + double x, y, z; + convert(lon, lat, alt, x, y, z, utmZone, hemisphere); + coords.push_back(Coord(x, y, z)); + } + imageListStream.close(); + + // Calculate average + double dx = 0.0, dy = 0.0; + double num = static_cast(coords.size()); + for (std::vector::iterator iter = coords.begin(); iter != coords.end(); ++iter) { + dx += iter->x/num; + dy += iter->y/num; + } + + dx = floor(dx); + dy = floor(dy); + + // Open output file + std::ofstream outputCoordStream(outputCoordFileName_.c_str()); + if (!outputCoordStream.good()) { + throw UtmExtractorException("Failed to openg " + outputCoordFileName_ + " for writing."); + } + outputCoordStream.precision(10); + + // Write coordinate file + outputCoordStream << "WGS84 UTM " << utmZone << hemisphere << std::endl; + outputCoordStream << dx << " " << dy << std::endl; + for (std::vector::iterator iter = coords.begin(); iter != coords.end(); ++iter) { + outputCoordStream << (iter->x - dx) << " " << (iter->y - dy) << " " << iter->z << std::endl; + } + + outputCoordStream.close(); +} + +bool UtmExtractor::convert(const double &lon, const double &lat, const double &alt, double &x, double &y, double &z, int &utmZone, char &hemisphere) +{ + x = y = z = 0.0; + + // Create WGS84 longitude/latitude coordinate system + projPJ pjLatLon = pj_init_plus("+proj=latlong +datum=WGS84"); + if (!pjLatLon) { + throw UtmExtractorException("Couldn't create WGS84 coordinate system with PROJ.4."); + return false; + } + + // Calculate UTM zone if it's set to magic 99 + // NOTE: Special UTM cases in Norway/Svalbard not supported here + if (utmZone == 99) { + utmZone = ((static_cast(floor((lon + 180.0)/6.0)) % 60) + 1); + if (lat < 0) + hemisphere = 'S'; + else + hemisphere = 'N'; + } + + std::ostringstream ostr; + ostr << utmZone; + if (hemisphere == 'S') + ostr << " +south"; + + // Create UTM coordinate system + projPJ pjUtm = pj_init_plus(("+proj=utm +datum=WGS84 +zone=" + ostr.str()).c_str()); + if (!pjUtm) { + throw UtmExtractorException("Couldn't create UTM coordinate system with PROJ.4."); + return false; + } + + // Convert to radians + x = lon * DEG_TO_RAD; + y = lat * DEG_TO_RAD; + z = alt; + + // Transform + int res = pj_transform(pjLatLon, pjUtm, 1, 1, &x, &y, &z); + if (res != 0) { + throw UtmExtractorException("Failed to transform coordinates"); + return false; + } + + return true; +} + +bool UtmExtractor::parsePosition(std::ifstream &jheadStream, double &lon, double &lat, double &alt) +{ + lon = lat = alt = 0.0; + + // Parse position + std::string str; + std::string latStr, lonStr, altStr; + while (std::getline(jheadStream, str)) + { + size_t index = str.find("GPS Latitude : "); + if (index != std::string::npos) + { + latStr = str.substr(index + 15); + size_t find = latStr.find_first_of("0123456789"); + if(std::string::npos == find) + { + throw UtmExtractorException("Image is missing GPS Latitude data"); + } + + } + index = str.find("GPS Longitude: "); + if (index != std::string::npos) + { + lonStr = str.substr(index + 15); + size_t find = lonStr.find_first_of("0123456789"); + if(std::string::npos == find) + { + throw UtmExtractorException("Image is missing GPS Latitude data"); + } + } + index = str.find("GPSAltitude"); + if (index != std::string::npos) + { + altStr = str.substr(index + 12); + size_t find = altStr.find_first_of("0123456789"); + if(std::string::npos == find) + { + throw UtmExtractorException("Image is missing GPS Latitude data"); + } + } + } + + if (lonStr.empty() || latStr.empty()) { + throw UtmExtractorException("No valid GPS position found"); + return false; + } + + // Parse longitude + std::string hemisphere; + double degrees, minutes, seconds; + std::istringstream istr(lonStr); + char degChar = 'd', minChar = 'm', secChar = 's'; + istr >> hemisphere >> degrees >> degChar >> minutes >> minChar >> seconds >> secChar; + lon = (hemisphere == "W" ? -1 : 1) * (degrees + minutes/60.0 + seconds/3600.0); + + // Parse latitude + istr.clear(); + istr.str(latStr); + istr >> hemisphere >> degrees >> degChar >> minutes >> minChar >> seconds >> secChar; + lat = (hemisphere == "S" ? -1 : 1) * (degrees + minutes/60.0 + seconds/3600.0); + + if (!altStr.empty()) + { + size_t index = altStr.find_last_of("="); + if (index != std::string::npos) + { + altStr = altStr.substr(index + 1); + istr.clear(); + istr.str(altStr); + + char dummyChar; + int nominator, denominator; + istr >> nominator; + istr >> dummyChar; + istr >> denominator; + + alt = static_cast(nominator)/static_cast(denominator); + } + } + + return true; +} + +void UtmExtractor::printHelp() +{ +log_.setIsPrintingInCout(true); + + log_ << "Purpose:\n"; + log_ << "Create a coordinate file containing the GPS positions of all cameras to be used later in the ODM toolchain for automatic georeferecing.\n"; + + log_ << "Usage:\n"; + log_ << "The program requires paths to a image list file, a image folder path and an output textfile to store the results.\n"; + + log_ << "The following flags are available:\n"; + log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n"; + log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output.\n\n"; + + log_ << "Parameters are specified as: \"- \", (without <>), and the following parameters are configurable:\n"; + log_ << "\"-imageListFile \" (mandatory)\n"; + log_ << "Path to the list containing the image names used in the bundle.out file.\n"; + + log_ << "\"-imagesPath \" (mandatory)\n"; + log_ << "Path folder containing all images in the imageListFile.\n"; + + log_ << "\"-outputCoordFile \" (mandatory)\n"; + log_ << "Path output textfile.\n"; + + log_.setIsPrintingInCout(false); +} diff --git a/odm_extract_utm/src/UtmExtractor.hpp b/odm_extract_utm/src/UtmExtractor.hpp new file mode 100644 index 000000000..64af0fa55 --- /dev/null +++ b/odm_extract_utm/src/UtmExtractor.hpp @@ -0,0 +1,96 @@ +#pragma once + +// Logging +#include "Logger.hpp" + +/*! +* \breif The Coord struct Class used in UtmExtractor to extract GPS positions from images and ODM output +*/ +struct Coord +{ + double x, y, z; + Coord(double ix, double iy, double iz) : x(ix), y(iy), z(iz) {} +}; + +class UtmExtractor +{ +public: + UtmExtractor(); + ~UtmExtractor(); + + /*! + * \brief run Runs the texturing functionality using the provided input arguments. + * For a list of the accepted arguments, please see the main page documentation or + * call the program with parameter "-help". + * \param argc Application argument count. + * \param argv Argument values. + * \return 0 if successful. + */ + int run (int argc, char **argv); + +private: + + /*! + * \brief parseArguments Parses command line arguments. + * \param argc Application argument count. + * \param argv Argument values. + */ + void parseArguments(int argc, char **argv); + + /*! + * \breif extractUtm Performs the extraction of coordinates inside the run function. + */ + void extractUtm(); + + /*! + * /brief Static method that converts a WGS84 longitude/latitude coordinate in decimal degrees to UTM. + * + * \param lon The longitude in decimal degrees (negative if western hemisphere). + * \param lat The latitude in decimal degrees (negative if southern hemisphere). + * \param alt The altitude in meters. + * \param x Output parameter, the easting UTM value in meters. + * \param y Output parameter, the northing UTM value in meters. + * \param utmZone Input or output parameter, the UTM zone. Set to 99 for automatic selection. + * \param hemisphere Input or output parameter, 'N' for norther hemisphere, 'S' for southern. Automatically selected if utmZone is 99. + * + * \returns True if successful (otherwise output parameters are 0) + */ + static bool convert(const double &lon, const double &lat, const double &alt, double &x, double &y, double &z, int &utmZone, char &hemisphere); + + /*! + * \brief Static method that parses a GPS position from jhead data. + * + * \param jheadDataStream Jhead data stream with EXIF information. + * \param lon Output parameter, the longitude in decimal degrees. + * \param lat Output parameter, the latitude in decimal degrees. + * \param alt Output parameter, the altitude in meters. + * + * \returns True if successful (otherwise output parameters are 0) + */ + static bool parsePosition(std::ifstream &jheadStream, double &lon, double &lat, double &alt); + + /*! + * \brief printHelp Prints help, explaining usage. Can be shown by calling the program with arguments: "-help". + */ + void printHelp(); + + std::string imageListFileName_; /**< Path to the image list. */ + std::string outputCoordFileName_; /**< Path to the file to store the output textfile. */ + std::string imagesPath_; /**< Path to the folder with all images in the image list. */ + + Logger log_; /**< Logging object. */ + std::string logFile_; /**< Path to store the log file. */ + +}; + +class UtmExtractorException : public std::exception +{ +public: + UtmExtractorException() : message("Error in OdmExtractUtm") {} + UtmExtractorException(std::string msgInit) : message("Error in OdmExtractUtm:\n" + msgInit) {} + ~UtmExtractorException() throw() {} + virtual const char* what() const throw() {return message.c_str(); } + +private: + std::string message; /**< The error message. */ +}; diff --git a/odm_extract_utm/src/main.cpp b/odm_extract_utm/src/main.cpp new file mode 100644 index 000000000..7e1a55e18 --- /dev/null +++ b/odm_extract_utm/src/main.cpp @@ -0,0 +1,9 @@ + + +#include "UtmExtractor.hpp" + +int main (int argc, char **argv) +{ + UtmExtractor utmExtractor; + utmExtractor.run(argc, argv); +} diff --git a/odm_georef/CMakeLists.txt b/odm_georef/CMakeLists.txt new file mode 100644 index 000000000..0d7a2e596 --- /dev/null +++ b/odm_georef/CMakeLists.txt @@ -0,0 +1,28 @@ +project(odm_georef) +cmake_minimum_required(VERSION 2.8) + +# Set pcl dir to the input spedified with option -DPCL_DIR="path" +set(PCL_DIR "PCL_DIR-NOTFOUND" CACHE "PCL_DIR" "Path to the pcl installation directory") +set(PROJ4_INCLUDE_DIR "/usr/include/" CACHE "PROJ4_INCLUDE_DIR" "Path to the proj4 inlcude directory") +set(PROJ4_LIBRARY "/usr/lib/libproj.so" CACHE "PROJ4_LIBRARY" "Path to the proj4 library directory") + +# Add compiler options. +add_definitions(-Wall -Wextra -Wconversion -pedantic) +#add_definitions(-pedantic -pedantic-errors -Wall -Wextra -Werror -Wfatal-errors -Wabi -Wctor-dtor-privacy -Wnon-virtual-dtor -Wreorder -Weffc++ -Wstrict-null-sentinel -Wnon-template-friend -Wold-style-cast -Woverloaded-virtual -Wpmf-conversions -Wsign-promo -Waddress -Warray-bounds -Wattributes -Wbuiltin-macro-redefined -Wc++0x-compat -Wcast-align -Wcast-qual -Wchar-subscripts -Wclobbered -Wcomment -Wconversion -Wcoverage-mismatch -Wdeprecated -Wdeprecated-declarations -Wdisabled-optimization -Wdiv-by-zero -Wempty-body -Wenum-compare -Wendif-labels -Wfatal-errors -Wfloat-equal -Wformat -Wformat=2 -Wformat-contains-nul -Wformat-extra-args -Wformat-nonliteral -Wformat-security -Wformat-y2k -Wignored-qualifiers -Winit-self -Wint-to-pointer-cast -Winvalid-offsetof -Winvalid-pch -Wlogical-op -Wmain -Wvariadic-macros -Wmissing-braces -Wmissing-field-initializers -Wmissing-include-dirs -Wmissing-noreturn -Wvla -Wmultichar -Wfatal-errors -Wnonnull -Woverflow -Woverlength-strings -Wpacked -Wpacked-bitfield-compat -Wparentheses -Wpointer-arith -Wredundant-decls -Wsequence-point -Wshadow -Wsign-compare -Wsign-conversion -Wstack-protector -Wstrict-overflow=5 -Wswitch -Wswitch-enum -Wsync-nand -Wvolatile-register-var -Wtrigraphs -Wtype-limits -Wuninitialized -Wunknown-pragmas -Wwrite-strings -Wpragmas -Wunreachable-code -Wunused -Wunused-function -Wunused-label -Wunused-parameter -Wunused-value -Wunused-variable -Wno-return-type) + +# Find pcl at the location specified by PCL_DIR +find_package(PCL 1.7 HINTS "${PCL_DIR}/share/pcl-1.7") + +# Add the PCL and Eigen include dirs. +# Necessary since the PCL_INCLUDE_DIR variable set bu find_package is broken.) +include_directories(${PCL_ROOT}/include/pcl-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}) +include_directories(${EIGEN_ROOT}) + +# Add source directory +aux_source_directory("./src" SRC_LIST) + +# Add exectuteable +add_executable(${PROJECT_NAME} ${SRC_LIST}) + +# Link +target_link_libraries(${PROJECT_NAME} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_SURFACE_LIBRARIES} ${PROJ4_LIBRARY}) diff --git a/odm_georef/src/FindTransform.cpp b/odm_georef/src/FindTransform.cpp new file mode 100644 index 000000000..a0f0a581c --- /dev/null +++ b/odm_georef/src/FindTransform.cpp @@ -0,0 +1,149 @@ +// This +#include "FindTransform.hpp" + +Vec3::Vec3(double x, double y, double z) :x_(x), y_(y), z_(z) +{ + +} +Vec3::Vec3(const Vec3 &o) : x_(o.x_), y_(o.y_), z_(o.z_) +{ + +} + +Vec3 Vec3::cross(Vec3 o) const +{ + Vec3 res; + res.x_ = y_*o.z_ - z_*o.y_; + res.y_ = z_*o.x_ - x_*o.z_; + res.z_ = x_*o.y_ - y_*o.x_; + return res; +} + +double Vec3::dot(Vec3 o) const +{ + return x_*o.x_ + y_*o.y_ + z_*o.z_; +} + +double Vec3::length() const +{ + return sqrt(x_*x_ + y_*y_ + z_*z_); +} + +Vec3 Vec3::norm() const +{ + Vec3 res; + double l = length(); + res.x_ = x_ / l; + res.y_ = y_ / l; + res.z_ = z_ / l; + return res; +} + +Vec3 Vec3::operator*(double d) const +{ + return Vec3(x_*d, y_*d, z_*d); +} + +Vec3 Vec3::operator+(Vec3 o) const +{ + return Vec3(x_ + o.x_, y_ + o.y_,z_ + o.z_); +} + +Vec3 Vec3::operator-(Vec3 o) const +{ + return Vec3(x_ - o.x_, y_ - o.y_,z_ - o.z_); +} + +OnMat3::OnMat3(Vec3 r1, Vec3 r2, Vec3 r3) : r1_(r1), r2_(r2), r3_(r3) +{ + c1_.x_ = r1_.x_; c2_.x_ = r1_.y_; c3_.x_ = r1_.z_; + c1_.y_ = r2_.x_; c2_.y_ = r2_.y_; c3_.y_ = r2_.z_; + c1_.z_ = r3_.x_; c2_.z_ = r3_.y_; c3_.z_ = r3_.z_; +} +OnMat3::OnMat3(const OnMat3 &o) : r1_(o.r1_), r2_(o.r2_), r3_(o.r3_) +{ + c1_.x_ = r1_.x_; c2_.x_ = r1_.y_; c3_.x_ = r1_.z_; + c1_.y_ = r2_.x_; c2_.y_ = r2_.y_; c3_.y_ = r2_.z_; + c1_.z_ = r3_.x_; c2_.z_ = r3_.y_; c3_.z_ = r3_.z_; +} + +double OnMat3::det() const +{ + return r1_.x_*r2_.y_*r3_.z_ + r1_.y_*r2_.z_*r3_.x_ + r1_.z_*r2_.x_*r3_.y_ - r1_.z_*r2_.y_*r3_.x_ - r1_.y_*r2_.x_*r3_.z_ - r1_.x_*r2_.z_*r3_.y_; +} + +OnMat3 OnMat3::transpose() const +{ + return OnMat3(Vec3(r1_.x_, r2_.x_, r3_.x_), Vec3(r1_.y_, r2_.y_, r3_.y_), Vec3(r1_.z_, r2_.z_, r3_.z_)); +} + +OnMat3 OnMat3::operator*(OnMat3 o) const +{ + return OnMat3( Vec3(r1_.dot(o.c1_), r1_.dot(o.c2_), r1_.dot(o.c3_)), + Vec3(r2_.dot(o.c1_), r2_.dot(o.c2_), r2_.dot(o.c3_)), + Vec3(r3_.dot(o.c1_), r3_.dot(o.c2_), r3_.dot(o.c3_))); +} + +Vec3 OnMat3::operator*(Vec3 o) +{ + return Vec3(r1_.dot(o), r2_.dot(o), r3_.dot(o)); +} + +Mat4::Mat4() +{ + r1c1_ = 1.0; r1c2_ = 0.0; r1c3_ = 0.0; r1c4_ = 0.0; + r2c1_ = 0.0; r2c2_ = 1.0; r2c3_ = 0.0; r2c4_ = 0.0; + r3c1_ = 0.0; r3c2_ = 0.0; r3c3_ = 1.0; r3c4_ = 0.0; + r4c1_ = 0.0; r4c2_ = 0.0; r4c3_ = 0.0; r4c4_ = 1.0; +} + +Mat4::Mat4(OnMat3 rotation, Vec3 translation, double scaling) +{ + r1c1_ = scaling * rotation.r1_.x_; r1c2_ = scaling * rotation.r1_.y_; r1c3_ = scaling * rotation.r1_.z_; r1c4_ = translation.x_; + r2c1_ = scaling * rotation.r2_.x_; r2c2_ = scaling * rotation.r2_.y_; r2c3_ = scaling * rotation.r2_.z_; r2c4_ = translation.y_; + r3c1_ = scaling * rotation.r3_.x_; r3c2_ = scaling * rotation.r3_.y_; r3c3_ = scaling * rotation.r3_.z_; r3c4_ = translation.z_; + r4c1_ = 0.0; r4c2_ = 0.0; r4c3_ = 0.0; r4c4_ = 1.0; +} + +Vec3 Mat4::operator*(Vec3 o) +{ + return Vec3( + r1c1_ * o.x_ + r1c2_* o.y_ + r1c3_* o.z_ + r1c4_, + r2c1_ * o.x_ + r2c2_* o.y_ + r2c3_* o.z_ + r2c4_, + r3c1_ * o.x_ + r3c2_* o.y_ + r3c3_* o.z_ + r3c4_ + ); +} + +void FindTransform::findTransform(Vec3 fromA, Vec3 fromB, Vec3 fromC, Vec3 toA, Vec3 toB, Vec3 toC) +{ + Vec3 a1 = toA; + Vec3 b1 = toB; + Vec3 c1 = toC; + Vec3 a2 = fromA; + Vec3 b2 = fromB; + Vec3 c2 = fromC; + + Vec3 y1 = (a1 - c1).cross(b1 - c1).norm(); + Vec3 z1 = (a1 - c1).norm(); + Vec3 x1 = y1.cross(z1); + + Vec3 y2 = (a2 - c2).cross(b2 - c2).norm(); + Vec3 z2 = (a2 - c2).norm(); + Vec3 x2 = y2.cross(z2); + OnMat3 mat1 = OnMat3(x1, y1, z1).transpose(); + OnMat3 mat2 = OnMat3(x2, y2, z2).transpose(); + + OnMat3 rotation = mat1 * mat2.transpose(); + Vec3 translation = c1 - c2; + + double scale = (a1 - c1).length() / (a2 - c2).length(); + + translation = rotation * c2 * (-scale) + c1; + Mat4 transformation(rotation, translation, scale); + transform_ = transformation; +} + +double FindTransform::error(Vec3 fromA, Vec3 toA) +{ + return (transform_*fromA - toA).length(); +} diff --git a/odm_georef/src/FindTransform.hpp b/odm_georef/src/FindTransform.hpp new file mode 100644 index 000000000..908e58a68 --- /dev/null +++ b/odm_georef/src/FindTransform.hpp @@ -0,0 +1,165 @@ +// C++ +#include +#include +#include +#include +#include + +/*! + * \brief Handles basic 3d vector math. + **/ +struct Vec3 +{ + Vec3(double x = 0.0, double y = 0.0, double z = 0.0); + Vec3(const Vec3 &o); + + double x_,y_,z_; /**< The x, y and z values of the vector. **/ + + /*! + * \brief cross The cross product between two vectors. + **/ + Vec3 cross(Vec3 o) const; + + /*! + * \brief dot The scalar product between two vectors. + **/ + double dot(Vec3 o) const; + + /*! + * \brief length The length of the vector. + **/ + double length() const; + + /*! + * \brief norm Returns a normalized version of this vector. + **/ + Vec3 norm() const; + + /*! + * \brief Scales this vector. + **/ + Vec3 operator*(double d) const; + + /*! + * \brief Addition between two vectors. + **/ + Vec3 operator+(Vec3 o) const; + + /*! + * \brief Subtraction between two vectors. + **/ + Vec3 operator-(Vec3 o) const; + + friend std::ostream & operator<<(std::ostream &os, Vec3 v) + { + return os << "[" << std::setprecision(8) << v.x_ << ", " << std::setprecision(4) << v.y_ << ", " << v.z_ << "]"; + } +}; + +/*! + * \brief Describes a 3d orthonormal matrix. + **/ +class OnMat3 +{ +public: + OnMat3(Vec3 r1, Vec3 r2, Vec3 r3); + OnMat3(const OnMat3 &o); + + Vec3 r1_; /**< The first row of the matrix. **/ + Vec3 r2_; /**< The second row of the matrix. **/ + Vec3 r3_; /**< The third row of the matrix. **/ + Vec3 c1_; /**< The first column of the matrix. **/ + Vec3 c2_; /**< The second column of the matrix. **/ + Vec3 c3_; /**< The third column of the matrix. **/ + + /*! + * \brief The determinant of the matrix. + **/ + double det() const; + + /*! + * \brief The transpose of the OnMat3 (equal to inverse). + **/ + OnMat3 transpose() const; + + /*! + * \brief Matrix multiplication between two ON matrices. + **/ + OnMat3 operator*(OnMat3 o) const; + + /*! + * \brief Right side multiplication with a 3d vector. + **/ + Vec3 operator*(Vec3 o); + + friend std::ostream & operator<<(std::ostream &os, OnMat3 m) + { + return os << "[" << std::endl << m.r1_ << std::endl << m.r2_ << std::endl << m.r3_ << std::endl << "]" << std::endl; + } +}; + +/*! + * \brief Describes an affine transformation. + **/ +class Mat4 +{ +public: + Mat4(); + Mat4(OnMat3 rotation, Vec3 translation, double scaling); + + /*! + * \brief Right side multiplication with a 3d vector. + **/ + Vec3 operator*(Vec3 o); + + double r1c1_; /**< Matrix element 0 0 **/ + double r1c2_; /**< Matrix element 0 1 **/ + double r1c3_; /**< Matrix element 0 2 **/ + double r1c4_; /**< Matrix element 0 3 **/ + double r2c1_; /**< Matrix element 1 0 **/ + double r2c2_; /**< Matrix element 1 1 **/ + double r2c3_; /**< Matrix element 1 2 **/ + double r2c4_; /**< Matrix element 1 3 **/ + double r3c1_; /**< Matrix element 2 0 **/ + double r3c2_; /**< Matrix element 2 1 **/ + double r3c3_; /**< Matrix element 2 2 **/ + double r3c4_; /**< Matrix element 2 3 **/ + double r4c1_; /**< Matrix element 3 0 **/ + double r4c2_; /**< Matrix element 3 1 **/ + double r4c3_; /**< Matrix element 3 2 **/ + double r4c4_; /**< Matrix element 3 3 **/ + + friend std::ostream & operator<<(std::ostream &os, Mat4 m) + { + std::stringstream ss; + ss.precision(8); + ss.setf(std::ios::fixed, std::ios::floatfield); + + ss << "[ " << m.r1c1_ << ",\t" << m.r1c2_ << ",\t" << m.r1c3_ << ",\t" << m.r1c4_ << " ]" << std::endl << + "[ " << m.r2c1_ << ",\t" << m.r2c2_ << ",\t" << m.r2c3_ << ",\t" << m.r2c4_ << " ]" << std::endl << + "[ " << m.r3c1_ << ",\t" << m.r3c2_ << ",\t" << m.r3c3_ << ",\t" << m.r3c4_ << " ]" << std::endl << + "[ " << m.r4c1_ << ",\t" << m.r4c2_ << ",\t" << m.r4c3_ << ",\t" << m.r4c4_ << " ]"; + + return os << ss.str(); + } + +}; + +class FindTransform +{ +public: + /*! + * \brief findTransform Generates an affine transform from the three 'from' vector to the three 'to' vectors. + * The transform is such that transform * fromA = toA, + * transform * fromB = toB, + * transform * fromC = toC, + **/ + void findTransform(Vec3 fromA, Vec3 fromB, Vec3 fromC, Vec3 toA, Vec3 toB, Vec3 toC); + + /*! + * \brief error Returns the distance beteween the 'from' and 'to' vectors, after the transform has been applied. + **/ + double error(Vec3 fromA, Vec3 toA); + + Mat4 transform_; /**< The affine transform. **/ +}; diff --git a/odm_georef/src/Georef.cpp b/odm_georef/src/Georef.cpp new file mode 100644 index 000000000..82f43674b --- /dev/null +++ b/odm_georef/src/Georef.cpp @@ -0,0 +1,530 @@ +// PCL +#include +#include + +// Modified PCL +#include "modifiedPclFunctions.hpp" + +// This +#include "Georef.hpp" + +std::ostream& operator<<(std::ostream &os, const GeorefSystem &geo) +{ + return os << geo.system_ << " " << static_cast(geo.falseEasting_) << " " << static_cast(geo.falseNorthing_); +} + +GeorefCamera::GeorefCamera() + :focalLength_(0.0), k1_(0.0), k2_(0.0), transform_(NULL), position_(NULL) +{ +} + +GeorefCamera::GeorefCamera(const GeorefCamera &other) + : focalLength_(other.focalLength_), k1_(other.k1_), k2_(other.k2_), + easting_(other.easting_), northing_(other.northing_), altitude_(other.altitude_), + transform_(NULL), position_(NULL) +{ + if(NULL != other.transform_) + { + transform_ = new Eigen::Affine3f(*other.transform_); + } + if(NULL != other.position_) + { + position_ = new Eigen::Vector3f(*other.position_); + } +} + +GeorefCamera::~GeorefCamera() +{ + if(NULL != transform_) + { + delete transform_; + transform_ = NULL; + } + if(NULL != position_) + { + delete position_; + position_ = NULL; + } +} + +void GeorefCamera::extractCamera(std::ifstream &bundleStream) +{ + // Extract intrinsic parameters. + bundleStream >> focalLength_ >> k1_ >> k2_; + + Eigen::Vector3f t; + Eigen::Matrix3f rot; + Eigen::Affine3f transform; + + bundleStream >> transform(0,0); // Read rotation (0,0) from bundle file + bundleStream >> transform(0,1); // Read rotation (0,1) from bundle file + bundleStream >> transform(0,2); // Read rotation (0,2) from bundle file + + bundleStream >> transform(1,0); // Read rotation (1,0) from bundle file + bundleStream >> transform(1,1); // Read rotation (1,1) from bundle file + bundleStream >> transform(1,2); // Read rotation (1,2) from bundle file + + bundleStream >> transform(2,0); // Read rotation (2,0) from bundle file + bundleStream >> transform(2,1); // Read rotation (2,1) from bundle file + bundleStream >> transform(2,2); // Read rotation (2,2) from bundle file + + bundleStream >> t(0); // Read translation (0,3) from bundle file + bundleStream >> t(1); // Read translation (1,3) from bundle file + bundleStream >> t(2); // Read translation (2,3) from bundle file + + rot = transform.matrix().topLeftCorner<3,3>(); + + // Calculate translation according to -R't and store in vector. + t = -rot.transpose()*t; + + transform(0,3) = t(0); + transform(1,3) = t(1); + transform(2,3) = t(2); + + // Set transform and position. + if(NULL != transform_) + { + delete transform_; + transform_ = NULL; + } + + transform_ = new Eigen::Affine3f(transform); + + if(NULL != position_) + { + delete position_; + position_ = NULL; + } + position_ = new Eigen::Vector3f(t); +} + +void GeorefCamera::extractCameraGeoref(std::istringstream &coordStream) +{ + coordStream >> easting_ >> northing_ >> altitude_; +} + +Vec3 GeorefCamera::getPos() +{ + return Vec3((*position_)(0),(*position_)(1),(*position_)(2)); +} + +Vec3 GeorefCamera::getReferencedPos() +{ + return Vec3(easting_,northing_,altitude_); +} + +std::ostream& operator<<(std::ostream &os, const GeorefCamera &cam) +{ + os << "Focal, k1, k2 : " << cam.focalLength_ << ", " << cam.k1_ << ", " << cam.k2_ << "\n"; + if(NULL != cam.transform_) + { + os << "Transform :\n" << cam.transform_->matrix() << "\n"; + } + else + { + os << "Transform :\nNULL\n"; + } + if(NULL != cam.position_) + { + os << "Position :\n" << cam.position_->matrix() << "\n"; + } + else + { + os << "Position :\nNULL\n"; + } + os << "east, north, alt : " << cam.easting_ << ", " << cam.northing_ << ", " << cam.altitude_ << '\n'; + return os; +} + +Georef::Georef() +{ + log_.setIsPrintingInCout(true); + + bundleFilename_ = ""; + coordFilename_ = ""; + inputObjFilename_ = ""; + outputObjFilename_ = ""; +} + +Georef::~Georef() +{ +} + +int Georef::run(int argc, char *argv[]) +{ + try + { + parseArguments(argc, argv); + makeGeoreferencedModel(); + } + catch (const GeorefException& e) + { + log_ << e.what() << "\n"; + log_.print(logFile_); + return EXIT_FAILURE; + } + catch (const std::exception& e) + { + log_ << "Error in Georef:\n"; + log_ << e.what() << "\n"; + log_.print(logFile_); + return EXIT_FAILURE; + } + catch (...) + { + log_ << "Unknown error, terminating:\n"; + log_.print(logFile_); + return EXIT_FAILURE; + } + + log_.print(logFile_); + + return EXIT_SUCCESS; +} + +void Georef::parseArguments(int argc, char *argv[]) +{ + bool outputSpecified = false; + + logFile_ = std::string(argv[0]) + "_log.txt"; + log_ << logFile_ << "\n"; + + // If no arguments were passed, print help. + if (argc == 1) + { + printHelp(); + } + + log_ << "Arguments given\n"; + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + log_ << argv[argIndex] << '\n'; + } + + log_ << '\n'; + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + // The argument to be parsed. + std::string argument = std::string(argv[argIndex]); + + if(argument == "-help") + { + printHelp(); + } + else if(argument == "-verbose") + { + log_.setIsPrintingInCout(true); + } + else if(argument == "-bundleFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw GeorefException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + bundleFilename_ = std::string(argv[argIndex]); + log_ << "Reading cameras from: " << bundleFilename_ << "\n"; + } + else if(argument == "-coordFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw GeorefException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + coordFilename_ = std::string(argv[argIndex]); + log_ << "Reading cameras georeferenced positions from: " << coordFilename_ << "\n"; + } + else if(argument == "-inputFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw GeorefException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + inputObjFilename_ = std::string(argv[argIndex]); + log_ << "Reading textured mesh from: " << inputObjFilename_ << "\n"; + } + else if(argument == "-outputFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw GeorefException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + outputObjFilename_ = std::string(argv[argIndex]); + log_ << "Writing output to: " << outputObjFilename_ << "\n"; + outputSpecified = true; + } + else + { + printHelp(); + throw GeorefException("Unrecognised argument '" + argument + "'"); + } + } + + if(!outputSpecified) + { + makeDefaultOutput(); + } +} + +void Georef::printHelp() +{ + bool printInCoutPop = log_.isPrintingInCout(); + log_.setIsPrintingInCout(true); + + log_ << "Georef.exe\n\n"; + + log_ << "Purpose:" << "\n"; + log_ << "Create an orthograpical photo from an oriented textured mesh." << "\n"; + + log_ << "Usage:" << "\n"; + log_ << "The program requires a path to a camera bundle file, a camera georeference coords file, and an input OBJ mesh file. All other input parameters are optional." << "\n\n"; + + log_ << "The following flags are available\n"; + log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n"; + log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output stream as well as in the log file.\n\n"; + + log_ << "Parameters are specified as: \"- \", (without <>), and the following parameters are configureable: " << "\n"; + log_ << "\"-bundleFile \" (mandatory)" << "\n"; + log_ << "\"Input cameras bundle file.\n\n"; + + log_ << "\"-coordFile \" (mandatory)" << "\n"; + log_ << "\"Input cameras geroreferenced coords file.\n\n"; + + log_ << "\"-inputFile \" (mandatory)" << "\n"; + log_ << "\"Input obj file that must contain a textured mesh.\n\n"; + + log_ << "\"-outputFile \" (optional, default _geo)" << "\n"; + log_ << "\"Output obj file that will contain the georeferenced texture mesh.\n\n"; + + log_.setIsPrintingInCout(printInCoutPop); +} + +void Georef::makeDefaultOutput() +{ + if(inputObjFilename_.empty()) + { + throw GeorefException("Tried to generate default ouptut file without having an input file."); + } + + std::string tmp = inputObjFilename_; + size_t findPos = tmp.find_last_of("."); + + if(std::string::npos == findPos) + { + throw GeorefException("Tried to generate default ouptut file, could not find .obj in the input file:\n\'"+inputObjFilename_+"\'"); + } + + tmp = tmp.substr(0, findPos); + + outputObjFilename_ = tmp + "_geo.obj"; + log_ << "Writing output to: " << outputObjFilename_ << "\n"; +} + +void Georef::makeGeoreferencedModel() +{ + // Read translations from bundle file + std::ifstream bundleStream(bundleFilename_.c_str()); + if (!bundleStream.good()) + { + throw GeorefException("Failed opening " + bundleFilename_ + " for reading." + '\n'); + } + + // Read Cameras. + std::string bundleLine; + std::getline(bundleStream, bundleLine); // Read past bundle version comment + int numCameras, numPoints; + bundleStream >> numCameras >> numPoints; + for (int i=0; i> georefSystem_.falseEasting_ >> georefSystem_.falseNorthing_; + } + + log_ << '\n'; + log_ << "Geographical reference system\n"; + log_ << georefSystem_ << '\n'; + + // The number of cameras in the coords file. + size_t nGeorefCameras = 0; + + // Read the georefernced position for all cameras. + while (std::getline(coordStream, coordString)) + { + if(nGeorefCameras >= cameras_.size()) + { + throw GeorefException("Error to many cameras in \'" + coordFilename_ + "\' coord file.\n"); + } + + std::istringstream istr(coordString); + cameras_[nGeorefCameras].extractCameraGeoref(istr); + + ++nGeorefCameras; + } + coordStream.close(); + + if(nGeorefCameras < cameras_.size()) + { + throw GeorefException("Not enough cameras in \'" + coordFilename_ + "\' coord file.\n"); + } + + // The optimal camera triplet. + size_t cam0, cam1, cam2; + + log_ << '\n'; + log_ << "Choosing optimal camera triplet...\n"; + chooseBestCameraTriplet(cam0, cam1, cam2); + log_ << "... optimal camera triplet chosen:\n"; + log_ << cam0 << ", " << cam1 << ", " << cam2 << '\n'; + log_ << '\n'; + FindTransform transFinal; + transFinal.findTransform(cameras_[cam0].getPos(), cameras_[cam1].getPos(), cameras_[cam2].getPos(), + cameras_[cam0].getReferencedPos(), cameras_[cam1].getReferencedPos(), cameras_[cam2].getReferencedPos()); + log_ << "Final transform:\n"; + log_ << transFinal.transform_ << '\n'; + + // The tranform used to move the chosen area into the ortho photo. + Eigen::Transform transform; + + transform(0, 0) = static_cast(transFinal.transform_.r1c1_); + transform(1, 0) = static_cast(transFinal.transform_.r2c1_); + transform(2, 0) = static_cast(transFinal.transform_.r3c1_); + transform(3, 0) = static_cast(transFinal.transform_.r4c1_); + + transform(0, 1) = static_cast(transFinal.transform_.r1c2_); + transform(1, 1) = static_cast(transFinal.transform_.r2c2_); + transform(2, 1) = static_cast(transFinal.transform_.r3c2_); + transform(3, 1) = static_cast(transFinal.transform_.r4c2_); + + transform(0, 2) = static_cast(transFinal.transform_.r1c3_); + transform(1, 2) = static_cast(transFinal.transform_.r2c3_); + transform(2, 2) = static_cast(transFinal.transform_.r3c3_); + transform(3, 2) = static_cast(transFinal.transform_.r4c3_); + + transform(0, 3) = static_cast(transFinal.transform_.r1c4_); + transform(1, 3) = static_cast(transFinal.transform_.r2c4_); + transform(2, 3) = static_cast(transFinal.transform_.r3c4_); + transform(3, 3) = static_cast(transFinal.transform_.r4c4_); + + log_ << '\n'; + log_ << "Reading mesh file...\n"; + // The textureds mesh.e + pcl::TextureMesh mesh; + pcl::io::loadOBJFile(inputObjFilename_, mesh); + log_ << ".. mesh file read.\n"; + + // Contains the vertices of the mesh. + pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (mesh.cloud, *meshCloud); + + log_ << '\n'; + log_ << "Applying transform to mesh...\n"; + // Move the mesh into position. + pcl::transformPointCloud(*meshCloud, *meshCloud, transform); + log_ << ".. mesh transformed.\n"; + + // Update the mesh. + pcl::toPCLPointCloud2 (*meshCloud, mesh.cloud); + + + // Iterate over each part of the mesh (one per material), to make texture file paths relative the .mtl file. + for(size_t t = 0; t < mesh.tex_materials.size(); ++t) + { + // The material of the current submesh. + pcl::TexMaterial& material = mesh.tex_materials[t]; + + size_t find = material.tex_file.find_last_of("/\\"); + if(std::string::npos != find) + { + material.tex_file = material.tex_file.substr(find + 1); + } + } + + log_ << '\n'; + log_ << "Saving mesh file to \'" << outputObjFilename_ << "\'...\n"; + saveOBJFile(outputObjFilename_, mesh, 8); + log_ << ".. mesh file saved.\n"; + + printGeorefSystem(); +} + +void Georef::chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2) +{ + double minTotError = std::numeric_limits::infinity(); + + for(size_t t = 0; t < cameras_.size(); ++t) + { + for(size_t s = t; s < cameras_.size(); ++s) + { + for(size_t p = s; p < cameras_.size(); ++p) + { + FindTransform trans; + trans.findTransform(cameras_[t].getPos(), cameras_[s].getPos(), cameras_[p].getPos(), + cameras_[t].getReferencedPos(), cameras_[s].getReferencedPos(), cameras_[p].getReferencedPos()); + + // The total error for the curren camera triplet. + double totError = 0.0; + + for(size_t r = 0; r < cameras_.size(); ++r) + { + totError += trans.error(cameras_[r].getPos(), cameras_[r].getReferencedPos()); + } + + if(minTotError > totError) + { + minTotError = totError; + cam0 = t; + cam1 = s; + cam2 = p; + } + } + } + } + + log_ << "Mean georeference error " << minTotError / static_cast(cameras_.size()) << '\n'; +} + +void Georef::printGeorefSystem() +{ + if(outputObjFilename_.empty()) + { + throw GeorefException("Output file path empty!."); + } + + std::string tmp = outputObjFilename_; + size_t findPos = tmp.find_last_of("."); + + if(std::string::npos == findPos) + { + throw GeorefException("Tried to generate default ouptut file, could not find .obj in the output file:\n\'"+outputObjFilename_+"\'"); + } + + tmp = tmp.substr(0, findPos); + + tmp = tmp + "_georef_system.txt"; + log_ << '\n'; + log_ << "Saving georeference system file to \'" << tmp << "\'...\n"; + std::ofstream geoStream(tmp.c_str()); + geoStream << georefSystem_ << std::endl; + geoStream.close(); + log_ << "... georeference system saved.\n"; +} + diff --git a/odm_georef/src/Georef.cpp~ b/odm_georef/src/Georef.cpp~ new file mode 100644 index 000000000..82f43674b --- /dev/null +++ b/odm_georef/src/Georef.cpp~ @@ -0,0 +1,530 @@ +// PCL +#include +#include + +// Modified PCL +#include "modifiedPclFunctions.hpp" + +// This +#include "Georef.hpp" + +std::ostream& operator<<(std::ostream &os, const GeorefSystem &geo) +{ + return os << geo.system_ << " " << static_cast(geo.falseEasting_) << " " << static_cast(geo.falseNorthing_); +} + +GeorefCamera::GeorefCamera() + :focalLength_(0.0), k1_(0.0), k2_(0.0), transform_(NULL), position_(NULL) +{ +} + +GeorefCamera::GeorefCamera(const GeorefCamera &other) + : focalLength_(other.focalLength_), k1_(other.k1_), k2_(other.k2_), + easting_(other.easting_), northing_(other.northing_), altitude_(other.altitude_), + transform_(NULL), position_(NULL) +{ + if(NULL != other.transform_) + { + transform_ = new Eigen::Affine3f(*other.transform_); + } + if(NULL != other.position_) + { + position_ = new Eigen::Vector3f(*other.position_); + } +} + +GeorefCamera::~GeorefCamera() +{ + if(NULL != transform_) + { + delete transform_; + transform_ = NULL; + } + if(NULL != position_) + { + delete position_; + position_ = NULL; + } +} + +void GeorefCamera::extractCamera(std::ifstream &bundleStream) +{ + // Extract intrinsic parameters. + bundleStream >> focalLength_ >> k1_ >> k2_; + + Eigen::Vector3f t; + Eigen::Matrix3f rot; + Eigen::Affine3f transform; + + bundleStream >> transform(0,0); // Read rotation (0,0) from bundle file + bundleStream >> transform(0,1); // Read rotation (0,1) from bundle file + bundleStream >> transform(0,2); // Read rotation (0,2) from bundle file + + bundleStream >> transform(1,0); // Read rotation (1,0) from bundle file + bundleStream >> transform(1,1); // Read rotation (1,1) from bundle file + bundleStream >> transform(1,2); // Read rotation (1,2) from bundle file + + bundleStream >> transform(2,0); // Read rotation (2,0) from bundle file + bundleStream >> transform(2,1); // Read rotation (2,1) from bundle file + bundleStream >> transform(2,2); // Read rotation (2,2) from bundle file + + bundleStream >> t(0); // Read translation (0,3) from bundle file + bundleStream >> t(1); // Read translation (1,3) from bundle file + bundleStream >> t(2); // Read translation (2,3) from bundle file + + rot = transform.matrix().topLeftCorner<3,3>(); + + // Calculate translation according to -R't and store in vector. + t = -rot.transpose()*t; + + transform(0,3) = t(0); + transform(1,3) = t(1); + transform(2,3) = t(2); + + // Set transform and position. + if(NULL != transform_) + { + delete transform_; + transform_ = NULL; + } + + transform_ = new Eigen::Affine3f(transform); + + if(NULL != position_) + { + delete position_; + position_ = NULL; + } + position_ = new Eigen::Vector3f(t); +} + +void GeorefCamera::extractCameraGeoref(std::istringstream &coordStream) +{ + coordStream >> easting_ >> northing_ >> altitude_; +} + +Vec3 GeorefCamera::getPos() +{ + return Vec3((*position_)(0),(*position_)(1),(*position_)(2)); +} + +Vec3 GeorefCamera::getReferencedPos() +{ + return Vec3(easting_,northing_,altitude_); +} + +std::ostream& operator<<(std::ostream &os, const GeorefCamera &cam) +{ + os << "Focal, k1, k2 : " << cam.focalLength_ << ", " << cam.k1_ << ", " << cam.k2_ << "\n"; + if(NULL != cam.transform_) + { + os << "Transform :\n" << cam.transform_->matrix() << "\n"; + } + else + { + os << "Transform :\nNULL\n"; + } + if(NULL != cam.position_) + { + os << "Position :\n" << cam.position_->matrix() << "\n"; + } + else + { + os << "Position :\nNULL\n"; + } + os << "east, north, alt : " << cam.easting_ << ", " << cam.northing_ << ", " << cam.altitude_ << '\n'; + return os; +} + +Georef::Georef() +{ + log_.setIsPrintingInCout(true); + + bundleFilename_ = ""; + coordFilename_ = ""; + inputObjFilename_ = ""; + outputObjFilename_ = ""; +} + +Georef::~Georef() +{ +} + +int Georef::run(int argc, char *argv[]) +{ + try + { + parseArguments(argc, argv); + makeGeoreferencedModel(); + } + catch (const GeorefException& e) + { + log_ << e.what() << "\n"; + log_.print(logFile_); + return EXIT_FAILURE; + } + catch (const std::exception& e) + { + log_ << "Error in Georef:\n"; + log_ << e.what() << "\n"; + log_.print(logFile_); + return EXIT_FAILURE; + } + catch (...) + { + log_ << "Unknown error, terminating:\n"; + log_.print(logFile_); + return EXIT_FAILURE; + } + + log_.print(logFile_); + + return EXIT_SUCCESS; +} + +void Georef::parseArguments(int argc, char *argv[]) +{ + bool outputSpecified = false; + + logFile_ = std::string(argv[0]) + "_log.txt"; + log_ << logFile_ << "\n"; + + // If no arguments were passed, print help. + if (argc == 1) + { + printHelp(); + } + + log_ << "Arguments given\n"; + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + log_ << argv[argIndex] << '\n'; + } + + log_ << '\n'; + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + // The argument to be parsed. + std::string argument = std::string(argv[argIndex]); + + if(argument == "-help") + { + printHelp(); + } + else if(argument == "-verbose") + { + log_.setIsPrintingInCout(true); + } + else if(argument == "-bundleFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw GeorefException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + bundleFilename_ = std::string(argv[argIndex]); + log_ << "Reading cameras from: " << bundleFilename_ << "\n"; + } + else if(argument == "-coordFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw GeorefException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + coordFilename_ = std::string(argv[argIndex]); + log_ << "Reading cameras georeferenced positions from: " << coordFilename_ << "\n"; + } + else if(argument == "-inputFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw GeorefException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + inputObjFilename_ = std::string(argv[argIndex]); + log_ << "Reading textured mesh from: " << inputObjFilename_ << "\n"; + } + else if(argument == "-outputFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw GeorefException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + outputObjFilename_ = std::string(argv[argIndex]); + log_ << "Writing output to: " << outputObjFilename_ << "\n"; + outputSpecified = true; + } + else + { + printHelp(); + throw GeorefException("Unrecognised argument '" + argument + "'"); + } + } + + if(!outputSpecified) + { + makeDefaultOutput(); + } +} + +void Georef::printHelp() +{ + bool printInCoutPop = log_.isPrintingInCout(); + log_.setIsPrintingInCout(true); + + log_ << "Georef.exe\n\n"; + + log_ << "Purpose:" << "\n"; + log_ << "Create an orthograpical photo from an oriented textured mesh." << "\n"; + + log_ << "Usage:" << "\n"; + log_ << "The program requires a path to a camera bundle file, a camera georeference coords file, and an input OBJ mesh file. All other input parameters are optional." << "\n\n"; + + log_ << "The following flags are available\n"; + log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n"; + log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output stream as well as in the log file.\n\n"; + + log_ << "Parameters are specified as: \"- \", (without <>), and the following parameters are configureable: " << "\n"; + log_ << "\"-bundleFile \" (mandatory)" << "\n"; + log_ << "\"Input cameras bundle file.\n\n"; + + log_ << "\"-coordFile \" (mandatory)" << "\n"; + log_ << "\"Input cameras geroreferenced coords file.\n\n"; + + log_ << "\"-inputFile \" (mandatory)" << "\n"; + log_ << "\"Input obj file that must contain a textured mesh.\n\n"; + + log_ << "\"-outputFile \" (optional, default _geo)" << "\n"; + log_ << "\"Output obj file that will contain the georeferenced texture mesh.\n\n"; + + log_.setIsPrintingInCout(printInCoutPop); +} + +void Georef::makeDefaultOutput() +{ + if(inputObjFilename_.empty()) + { + throw GeorefException("Tried to generate default ouptut file without having an input file."); + } + + std::string tmp = inputObjFilename_; + size_t findPos = tmp.find_last_of("."); + + if(std::string::npos == findPos) + { + throw GeorefException("Tried to generate default ouptut file, could not find .obj in the input file:\n\'"+inputObjFilename_+"\'"); + } + + tmp = tmp.substr(0, findPos); + + outputObjFilename_ = tmp + "_geo.obj"; + log_ << "Writing output to: " << outputObjFilename_ << "\n"; +} + +void Georef::makeGeoreferencedModel() +{ + // Read translations from bundle file + std::ifstream bundleStream(bundleFilename_.c_str()); + if (!bundleStream.good()) + { + throw GeorefException("Failed opening " + bundleFilename_ + " for reading." + '\n'); + } + + // Read Cameras. + std::string bundleLine; + std::getline(bundleStream, bundleLine); // Read past bundle version comment + int numCameras, numPoints; + bundleStream >> numCameras >> numPoints; + for (int i=0; i> georefSystem_.falseEasting_ >> georefSystem_.falseNorthing_; + } + + log_ << '\n'; + log_ << "Geographical reference system\n"; + log_ << georefSystem_ << '\n'; + + // The number of cameras in the coords file. + size_t nGeorefCameras = 0; + + // Read the georefernced position for all cameras. + while (std::getline(coordStream, coordString)) + { + if(nGeorefCameras >= cameras_.size()) + { + throw GeorefException("Error to many cameras in \'" + coordFilename_ + "\' coord file.\n"); + } + + std::istringstream istr(coordString); + cameras_[nGeorefCameras].extractCameraGeoref(istr); + + ++nGeorefCameras; + } + coordStream.close(); + + if(nGeorefCameras < cameras_.size()) + { + throw GeorefException("Not enough cameras in \'" + coordFilename_ + "\' coord file.\n"); + } + + // The optimal camera triplet. + size_t cam0, cam1, cam2; + + log_ << '\n'; + log_ << "Choosing optimal camera triplet...\n"; + chooseBestCameraTriplet(cam0, cam1, cam2); + log_ << "... optimal camera triplet chosen:\n"; + log_ << cam0 << ", " << cam1 << ", " << cam2 << '\n'; + log_ << '\n'; + FindTransform transFinal; + transFinal.findTransform(cameras_[cam0].getPos(), cameras_[cam1].getPos(), cameras_[cam2].getPos(), + cameras_[cam0].getReferencedPos(), cameras_[cam1].getReferencedPos(), cameras_[cam2].getReferencedPos()); + log_ << "Final transform:\n"; + log_ << transFinal.transform_ << '\n'; + + // The tranform used to move the chosen area into the ortho photo. + Eigen::Transform transform; + + transform(0, 0) = static_cast(transFinal.transform_.r1c1_); + transform(1, 0) = static_cast(transFinal.transform_.r2c1_); + transform(2, 0) = static_cast(transFinal.transform_.r3c1_); + transform(3, 0) = static_cast(transFinal.transform_.r4c1_); + + transform(0, 1) = static_cast(transFinal.transform_.r1c2_); + transform(1, 1) = static_cast(transFinal.transform_.r2c2_); + transform(2, 1) = static_cast(transFinal.transform_.r3c2_); + transform(3, 1) = static_cast(transFinal.transform_.r4c2_); + + transform(0, 2) = static_cast(transFinal.transform_.r1c3_); + transform(1, 2) = static_cast(transFinal.transform_.r2c3_); + transform(2, 2) = static_cast(transFinal.transform_.r3c3_); + transform(3, 2) = static_cast(transFinal.transform_.r4c3_); + + transform(0, 3) = static_cast(transFinal.transform_.r1c4_); + transform(1, 3) = static_cast(transFinal.transform_.r2c4_); + transform(2, 3) = static_cast(transFinal.transform_.r3c4_); + transform(3, 3) = static_cast(transFinal.transform_.r4c4_); + + log_ << '\n'; + log_ << "Reading mesh file...\n"; + // The textureds mesh.e + pcl::TextureMesh mesh; + pcl::io::loadOBJFile(inputObjFilename_, mesh); + log_ << ".. mesh file read.\n"; + + // Contains the vertices of the mesh. + pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (mesh.cloud, *meshCloud); + + log_ << '\n'; + log_ << "Applying transform to mesh...\n"; + // Move the mesh into position. + pcl::transformPointCloud(*meshCloud, *meshCloud, transform); + log_ << ".. mesh transformed.\n"; + + // Update the mesh. + pcl::toPCLPointCloud2 (*meshCloud, mesh.cloud); + + + // Iterate over each part of the mesh (one per material), to make texture file paths relative the .mtl file. + for(size_t t = 0; t < mesh.tex_materials.size(); ++t) + { + // The material of the current submesh. + pcl::TexMaterial& material = mesh.tex_materials[t]; + + size_t find = material.tex_file.find_last_of("/\\"); + if(std::string::npos != find) + { + material.tex_file = material.tex_file.substr(find + 1); + } + } + + log_ << '\n'; + log_ << "Saving mesh file to \'" << outputObjFilename_ << "\'...\n"; + saveOBJFile(outputObjFilename_, mesh, 8); + log_ << ".. mesh file saved.\n"; + + printGeorefSystem(); +} + +void Georef::chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2) +{ + double minTotError = std::numeric_limits::infinity(); + + for(size_t t = 0; t < cameras_.size(); ++t) + { + for(size_t s = t; s < cameras_.size(); ++s) + { + for(size_t p = s; p < cameras_.size(); ++p) + { + FindTransform trans; + trans.findTransform(cameras_[t].getPos(), cameras_[s].getPos(), cameras_[p].getPos(), + cameras_[t].getReferencedPos(), cameras_[s].getReferencedPos(), cameras_[p].getReferencedPos()); + + // The total error for the curren camera triplet. + double totError = 0.0; + + for(size_t r = 0; r < cameras_.size(); ++r) + { + totError += trans.error(cameras_[r].getPos(), cameras_[r].getReferencedPos()); + } + + if(minTotError > totError) + { + minTotError = totError; + cam0 = t; + cam1 = s; + cam2 = p; + } + } + } + } + + log_ << "Mean georeference error " << minTotError / static_cast(cameras_.size()) << '\n'; +} + +void Georef::printGeorefSystem() +{ + if(outputObjFilename_.empty()) + { + throw GeorefException("Output file path empty!."); + } + + std::string tmp = outputObjFilename_; + size_t findPos = tmp.find_last_of("."); + + if(std::string::npos == findPos) + { + throw GeorefException("Tried to generate default ouptut file, could not find .obj in the output file:\n\'"+outputObjFilename_+"\'"); + } + + tmp = tmp.substr(0, findPos); + + tmp = tmp + "_georef_system.txt"; + log_ << '\n'; + log_ << "Saving georeference system file to \'" << tmp << "\'...\n"; + std::ofstream geoStream(tmp.c_str()); + geoStream << georefSystem_ << std::endl; + geoStream.close(); + log_ << "... georeference system saved.\n"; +} + diff --git a/odm_georef/src/Georef.hpp b/odm_georef/src/Georef.hpp new file mode 100644 index 000000000..440961568 --- /dev/null +++ b/odm_georef/src/Georef.hpp @@ -0,0 +1,150 @@ +#pragma once + +// C++ +#include +#include +#include + +// PCL +#include +#include + +// Logger +#include "Logger.hpp" + +// Transformation +#include "FindTransform.hpp" + +/*! + * \brief The GeorefSystem struct is used to store information about a georeference system. + */ +struct GeorefSystem +{ + std::string system_; /**< The name of the system. **/ + double falseEasting_; /**< The false easting of the cameras. **/ + double falseNorthing_; /**< The false northing of the cameras. **/ + + friend std::ostream& operator<<(std::ostream &os, const GeorefSystem &geo); +}; + +/*! + * \brief The GeorefCamera struct is used to store information about a camera. + */ +struct GeorefCamera +{ + GeorefCamera(); + GeorefCamera(const GeorefCamera &other); + ~GeorefCamera(); + + /*! + * \brief extractCamera Extracts a camera's intrinsic and extrinsic parameters from a stream. + */ + void extractCamera(std::ifstream &bundleStream); + + /*! + * \brief extractCameraGeoref Extracts a camera's world position from a stream. + */ + void extractCameraGeoref(std::istringstream &coordStream); + + /*! + * \brief getReferencedPos Get the local position of the camera. + */ + Vec3 getPos(); + + /*! + * \brief getReferencedPos Get the georeferenced position of the camera. + */ + Vec3 getReferencedPos(); + + double focalLength_; /**< The focal length of the camera. */ + double k1_; /**< The k1 lens distortion parameter. **/ + double k2_; /**< The k2 lens distortion parameter. **/ + + double easting_; /**< The easting of the camera. **/ + double northing_; /**< The northing of the camera. **/ + double altitude_; /**< The altitude of the camera. **/ + + Eigen::Affine3f* transform_; /**< The rotation of the camera. **/ + Eigen::Vector3f* position_; /**< The position of the camera. **/ + + friend std::ostream& operator<<(std::ostream &os, const GeorefCamera &cam); +}; + +/*! + * \brief The OdmOrthoPhoto class is used to transform a mesh into a georeferenced system. + * The class reads camera positions from a bundle file. + * The class reads the georefenced camera positions from a coords file. + * The class reads a textured mesh from an OBJ-file. + * The class writes the georeferenced textured mesh to an OBJ-file. + * The class uses file read and write from pcl. + */ +class Georef +{ +public: + Georef(); + ~Georef(); + + int run(int argc, char* argv[]); + +private: + + /*! + * \brief parseArguments Parses command line arguments. + * \param argc Application argument count. + * \param argv Argument values. + */ + void parseArguments(int argc, char* argv[]); + + /*! + * \brief printHelp Prints help, explaining usage. Can be shown by calling the program with argument: "-help". + */ + void printHelp(); + + /*! + * \brief makeDefaultOutput Setup the output file name given the input file name. + */ + void makeDefaultOutput(); + + /*! + * \brief makeGeoreferencedModel Makes the input file georeferenced and saves it to the output file. + */ + void makeGeoreferencedModel(); + + /*! + * \brief chooseBestCameraTriplet Chooses the best triplet of cameras to use when makin gthe model georeferenced. + */ + void chooseBestCameraTriplet(size_t &cam0, size_t &cam1, size_t &cam2); + + /*! + * \brief printGeorefSystem Prints a file containing information about the georeference system, next to the ouptut file. + **/ + void printGeorefSystem(); + + Logger log_; /**< Logging object. */ + std::string logFile_; /**< The path to the output log file. */ + + std::string bundleFilename_; /**< The path to the cameras bundle file. **/ + std::string coordFilename_; /**< The path to the cameras georeference file. **/ + std::string inputObjFilename_; /**< The path to the input mesh obj file. **/ + std::string outputObjFilename_; /**< The path to the output mesh obj file. **/ + + std::vector cameras_; /**< A vector of all cameras. **/ + + GeorefSystem georefSystem_; /**< Contains the georeference system. **/ +}; + +/*! + * \brief The Georef class + */ +class GeorefException : public std::exception +{ + +public: + GeorefException() : message("Error in Georef") {} + GeorefException(std::string msgInit) : message("Error in Georef:\n" + msgInit) {} + ~GeorefException() throw() {} + virtual const char* what() const throw() {return message.c_str(); } + +private: + std::string message; /**< The error message **/ +}; diff --git a/odm_georef/src/Logger.cpp b/odm_georef/src/Logger.cpp new file mode 100644 index 000000000..46b96fd72 --- /dev/null +++ b/odm_georef/src/Logger.cpp @@ -0,0 +1,31 @@ +#include "Logger.hpp" + + +Logger::Logger(bool isPrintingInCout) : isPrintingInCout_(isPrintingInCout) +{ + +} + +Logger::~Logger() +{ + +} + +void Logger::print(std::string filePath) +{ + std::ofstream file(filePath.c_str(), std::ios::binary); + file << logStream_.str(); + file.close(); +} + +bool Logger::isPrintingInCout() const +{ + return isPrintingInCout_; +} + +void Logger::setIsPrintingInCout(bool isPrintingInCout) +{ + isPrintingInCout_ = isPrintingInCout; +} + + diff --git a/odm_georef/src/Logger.hpp b/odm_georef/src/Logger.hpp new file mode 100644 index 000000000..61520146e --- /dev/null +++ b/odm_georef/src/Logger.hpp @@ -0,0 +1,68 @@ +#pragma once + +// STL +#include +#include +#include +#include + +/*! + * \brief The Logger class is used to store program messages in a log file. + * \details By using the << operator while printInCout is set, the class writes both to + * cout and to file, if the flag is not set, output is written to file only. + */ +class Logger +{ +public: + /*! + * \brief Logger Contains functionality for printing and displaying log information. + * \param printInCout Flag toggling if operator << also writes to cout. + */ + Logger(bool isPrintingInCout = true); + + /*! + * \brief Destructor. + */ + ~Logger(); + + /*! + * \brief print Prints the contents of the log to file. + * \param filePath Path specifying where to write the log. + */ + void print(std::string filePath); + + /*! + * \brief isPrintingInCout Check if console printing flag is set. + * \return Console printing flag. + */ + bool isPrintingInCout() const; + + /*! + * \brief setIsPrintingInCout Set console printing flag. + * \param isPrintingInCout Value, if true, messages added to the log are also printed in cout. + */ + void setIsPrintingInCout(bool isPrintingInCout); + + /*! + * Operator for printing messages to log and in the standard output stream if desired. + */ + template + friend Logger& operator<< (Logger &log, T t) + { + // If console printing is enabled. + if (log.isPrintingInCout_) + { + std::cout << t; + std::cout.flush(); + } + // Write to log. + log.logStream_ << t; + + return log; + } + +private: + bool isPrintingInCout_; /*!< If flag is set, log is printed in cout and written to the log. */ + + std::stringstream logStream_; /*!< Stream for storing the log. */ +}; diff --git a/odm_georef/src/main.cpp b/odm_georef/src/main.cpp new file mode 100644 index 000000000..eef4096d4 --- /dev/null +++ b/odm_georef/src/main.cpp @@ -0,0 +1,8 @@ +#include "Georef.hpp" + +int main(int argc, char* argv[]) +{ + Georef ref; + return ref.run(argc, argv); +} + diff --git a/odm_georef/src/modifiedPclFunctions.cpp b/odm_georef/src/modifiedPclFunctions.cpp new file mode 100644 index 000000000..eafa94068 --- /dev/null +++ b/odm_georef/src/modifiedPclFunctions.cpp @@ -0,0 +1,336 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2012-, Open Perception, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ + +#include "modifiedPclFunctions.hpp" + +int saveOBJFile(const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision) +{ + if (tex_mesh.cloud.data.empty ()) + { + PCL_ERROR ("[pcl::io::saveOBJFile] Input point cloud has no data!\n"); + return (-1); + } + + // Open file + std::ofstream fs; + fs.precision (precision); + fs.open (file_name.c_str ()); + + // Define material file + std::string mtl_file_name = file_name.substr (0, file_name.find_last_of (".")) + ".mtl"; + // Strip path for "mtllib" command + std::string mtl_file_name_nopath = mtl_file_name; + //std::cout << mtl_file_name_nopath << std::endl; + mtl_file_name_nopath.erase (0, mtl_file_name.find_last_of ('/') + 1); + + /* Write 3D information */ + // number of points + int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height; + int point_size = tex_mesh.cloud.data.size () / nr_points; + + // mesh size + int nr_meshes = tex_mesh.tex_polygons.size (); + // number of faces for header + int nr_faces = 0; + for (int m = 0; m < nr_meshes; ++m) + nr_faces += tex_mesh.tex_polygons[m].size (); + + // Write the header information + fs << "####" << std::endl; + fs << "# OBJ dataFile simple version. File name: " << file_name << std::endl; + fs << "# Vertices: " << nr_points << std::endl; + fs << "# Faces: " < 0) + f_idx += tex_mesh.tex_polygons[m-1].size (); + + if(tex_mesh.tex_materials.size() !=0) + { + fs << "# The material will be used for mesh " << m << std::endl; + //TODO pbl here with multi texture and unseen faces + fs << "usemtl " << tex_mesh.tex_materials[m].tex_name << std::endl; + fs << "# Faces" << std::endl; + } + for (size_t i = 0; i < tex_mesh.tex_polygons[m].size(); ++i) + { + // Write faces with "f" + fs << "f"; + size_t j = 0; + // There's one UV per vertex per face, i.e., the same vertex can have + // different UV depending on the face. + for (j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) + { + unsigned int idx = tex_mesh.tex_polygons[m][i].vertices[j] + 1; + fs << " " << idx + << "/" << 3*(i+f_idx) +j+1; + //<< "/" << idx; // vertex index in obj file format starting with 1 + } + fs << std::endl; + } + //PCL_INFO ("%d faces in mesh %d \n", tex_mesh.tex_polygons[m].size () , m); + fs << "# "<< tex_mesh.tex_polygons[m].size() << " faces in mesh " << m << std::endl; + } + fs << "# End of File"; + + // Close obj file + //PCL_INFO ("Closing obj file\n"); + fs.close (); + + /* Write material defination for OBJ file*/ + // Open file + //PCL_INFO ("Writing material files\n"); + //dont do it if no material to write + if(tex_mesh.tex_materials.size() ==0) + return (0); + + std::ofstream m_fs; + m_fs.precision (precision); + m_fs.open (mtl_file_name.c_str ()); + //std::cout << "MTL file is located at_ " << mtl_file_name << std::endl; + // default + m_fs << "#" << std::endl; + m_fs << "# Wavefront material file" << std::endl; + m_fs << "#" << std::endl; + for(int m = 0; m < nr_meshes; ++m) + { + m_fs << "newmtl " << tex_mesh.tex_materials[m].tex_name << std::endl; + m_fs << "Ka "<< tex_mesh.tex_materials[m].tex_Ka.r << " " << tex_mesh.tex_materials[m].tex_Ka.g << " " << tex_mesh.tex_materials[m].tex_Ka.b << std::endl; // defines the ambient color of the material to be (r,g,b). + m_fs << "Kd "<< tex_mesh.tex_materials[m].tex_Kd.r << " " << tex_mesh.tex_materials[m].tex_Kd.g << " " << tex_mesh.tex_materials[m].tex_Kd.b << std::endl; // defines the diffuse color of the material to be (r,g,b). + m_fs << "Ks "<< tex_mesh.tex_materials[m].tex_Ks.r << " " << tex_mesh.tex_materials[m].tex_Ks.g << " " << tex_mesh.tex_materials[m].tex_Ks.b << std::endl; // defines the specular color of the material to be (r,g,b). This color shows up in highlights. + m_fs << "d " << tex_mesh.tex_materials[m].tex_d << std::endl; // defines the transparency of the material to be alpha. + m_fs << "Ns "<< tex_mesh.tex_materials[m].tex_Ns << std::endl; // defines the shininess of the material to be s. + m_fs << "illum "<< tex_mesh.tex_materials[m].tex_illum << std::endl; // denotes the illumination model used by the material. + // illum = 1 indicates a flat material with no specular highlights, so the value of Ks is not used. + // illum = 2 denotes the presence of specular highlights, and so a specification for Ks is required. + m_fs << "map_Kd " << tex_mesh.tex_materials[m].tex_file << std::endl; + m_fs << "###" << std::endl; + } + m_fs.close (); + return (0); +} + +bool getPixelCoordinates(const pcl::PointXYZ &pt, const pcl::TextureMapping::Camera &cam, pcl::PointXY &UV_coordinates) +{ + if (pt.z > 0) + { + // compute image center and dimension + double sizeX = cam.width; + double sizeY = cam.height; + double cx, cy; + if (cam.center_w > 0) + cx = cam.center_w; + else + cx = sizeX / 2.0; + if (cam.center_h > 0) + cy = cam.center_h; + else + cy = sizeY / 2.0; + + double focal_x, focal_y; + if (cam.focal_length_w > 0) + focal_x = cam.focal_length_w; + else + focal_x = cam.focal_length; + if (cam.focal_length_h > 0) + focal_y = cam.focal_length_h; + else + focal_y = cam.focal_length; + + // project point on camera's image plane + UV_coordinates.x = static_cast ((focal_x * (pt.x / pt.z) + cx)); //horizontal + UV_coordinates.y = static_cast ((focal_y * (pt.y / pt.z) + cy)); //vertical + + // point is visible! + if (UV_coordinates.x >= 15.0 && UV_coordinates.x <= (sizeX - 15.0) && UV_coordinates.y >= 15.0 && UV_coordinates.y <= (sizeY - 15.0)) + { + return (true); // point was visible by the camera + } + } + + // point is NOT visible by the camera + UV_coordinates.x = -1.0f; + UV_coordinates.y = -1.0f; + return (false); // point was not visible by the camera +} + +bool isFaceProjected (const pcl::TextureMapping::Camera &camera, const pcl::PointXYZ &p1, const pcl::PointXYZ &p2, const pcl::PointXYZ &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3) +{ + return (getPixelCoordinates(p1, camera, proj1) && getPixelCoordinates(p2, camera, proj2) && getPixelCoordinates(p3, camera, proj3)); +} + +void getTriangleCircumscribedCircleCentroid( const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius) +{ + // compute centroid's coordinates (translate back to original coordinates) + circumcenter.x = static_cast (p1.x + p2.x + p3.x ) / 3; + circumcenter.y = static_cast (p1.y + p2.y + p3.y ) / 3; + double r1 = (circumcenter.x - p1.x) * (circumcenter.x - p1.x) + (circumcenter.y - p1.y) * (circumcenter.y - p1.y) ; + double r2 = (circumcenter.x - p2.x) * (circumcenter.x - p2.x) + (circumcenter.y - p2.y) * (circumcenter.y - p2.y) ; + double r3 = (circumcenter.x - p3.x) * (circumcenter.x - p3.x) + (circumcenter.y - p3.y) * (circumcenter.y - p3.y) ; + + // radius + radius = std::sqrt( std::max( r1, std::max( r2, r3) )) ; +} + +bool checkPointInsideTriangle(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt) +{ + // Compute vectors + Eigen::Vector2d v0, v1, v2; + v0(0) = p3.x - p1.x; v0(1) = p3.y - p1.y; // v0= C - A + v1(0) = p2.x - p1.x; v1(1) = p2.y - p1.y; // v1= B - A + v2(0) = pt.x - p1.x; v2(1) = pt.y - p1.y; // v2= P - A + + // Compute dot products + double dot00 = v0.dot(v0); // dot00 = dot(v0, v0) + double dot01 = v0.dot(v1); // dot01 = dot(v0, v1) + double dot02 = v0.dot(v2); // dot02 = dot(v0, v2) + double dot11 = v1.dot(v1); // dot11 = dot(v1, v1) + double dot12 = v1.dot(v2); // dot12 = dot(v1, v2) + + // Compute barycentric coordinates + double invDenom = 1.0 / (dot00*dot11 - dot01*dot01); + double u = (dot11*dot02 - dot01*dot12) * invDenom; + double v = (dot00*dot12 - dot01*dot02) * invDenom; + + // Check if point is in triangle + return ((u >= 0) && (v >= 0) && (u + v < 1)); +} diff --git a/odm_georef/src/modifiedPclFunctions.hpp b/odm_georef/src/modifiedPclFunctions.hpp new file mode 100644 index 000000000..5739df146 --- /dev/null +++ b/odm_georef/src/modifiedPclFunctions.hpp @@ -0,0 +1,20 @@ +#pragma once + +// STL +#include +#include + +// PCL +#include +#include +#include + +int saveOBJFile(const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision); + +bool getPixelCoordinates(const pcl::PointXYZ &pt, const pcl::TextureMapping::Camera &cam, pcl::PointXY &UV_coordinates); + +bool isFaceProjected (const pcl::TextureMapping::Camera &camera, const pcl::PointXYZ &p1, const pcl::PointXYZ &p2, const pcl::PointXYZ &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3); + +void getTriangleCircumscribedCircleCentroid(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius); + +bool checkPointInsideTriangle(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt); diff --git a/odm_meshing/CMakeLists.txt b/odm_meshing/CMakeLists.txt new file mode 100644 index 000000000..f73fa8460 --- /dev/null +++ b/odm_meshing/CMakeLists.txt @@ -0,0 +1,26 @@ +project(odm_meshing) +cmake_minimum_required(VERSION 2.8) + +# Set pcl dir to the input spedified with option -DPCL_DIR="path" +set(PCL_DIR "PCL_DIR-NOTFOUND" CACHE "PCL_DIR" "Path to the pcl installation directory") + +# Add compiler options. +add_definitions(-Wall -Wextra) + +# Find pcl at the location specified by PCL_DIR +find_package(PCL 1.7 HINTS "${PCL_DIR}/share/pcl-1.7") + +# Add the PCL and Eigen include dirs. +# Necessary since the PCL_INCLUDE_DIR variable set by find_package is broken.) +include_directories(${PCL_ROOT}/include/pcl-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}) +include_directories(${EIGEN_ROOT}) + +# Add source directory +aux_source_directory("./src" SRC_LIST) + +# Add exectuteable +add_executable(${PROJECT_NAME} ${SRC_LIST}) + +# Link +target_link_libraries(odm_meshing ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_SURFACE_LIBRARIES}) + diff --git a/odm_meshing/src/Logger.cpp b/odm_meshing/src/Logger.cpp new file mode 100644 index 000000000..a6c81a8b5 --- /dev/null +++ b/odm_meshing/src/Logger.cpp @@ -0,0 +1,31 @@ +#include "Logger.hpp" + + +Logger::Logger(bool isPrintingInCout) : isPrintingInCout_(isPrintingInCout) +{ + +} + +Logger::~Logger() +{ + +} + +void Logger::printToFile(std::string filePath) +{ + std::ofstream file(filePath.c_str(), std::ios::binary); + file << logStream_.str(); + file.close(); +} + +bool Logger::isPrintingInCout() const +{ + return isPrintingInCout_; +} + +void Logger::setIsPrintingInCout(bool isPrintingInCout) +{ + isPrintingInCout_ = isPrintingInCout; +} + + diff --git a/odm_meshing/src/Logger.hpp b/odm_meshing/src/Logger.hpp new file mode 100644 index 000000000..31c5538cb --- /dev/null +++ b/odm_meshing/src/Logger.hpp @@ -0,0 +1,68 @@ +#pragma once + +// STL +#include +#include +#include +#include + +/*! + * \brief The Logger class is used to store program messages in a log file. + * \details By using the << operator while printInCout is set, the class writes both to + * cout and to file, if the flag is not set, output is written to file only. + */ +class Logger +{ +public: + /*! + * \brief Logger Contains functionality for printing and displaying log information. + * \param printInCout Flag toggling if operator << also writes to cout. + */ + Logger(bool isPrintingInCout = true); + + /*! + * \brief Destructor. + */ + ~Logger(); + + /*! + * \brief print Prints the contents of the log to file. + * \param filePath Path specifying where to write the log. + */ + void printToFile(std::string filePath); + + /*! + * \brief isPrintingInCout Check if console printing flag is set. + * \return Console printing flag. + */ + bool isPrintingInCout() const; + + /*! + * \brief setIsPrintingInCout Set console printing flag. + * \param isPrintingInCout Value, if true, messages added to the log are also printed in cout. + */ + void setIsPrintingInCout(bool isPrintingInCout); + + /*! + * Operator for printing messages to log and in the standard output stream if desired. + */ + template + friend Logger& operator<< (Logger &log, T t) + { + // If console printing is enabled. + if (log.isPrintingInCout_) + { + std::cout << t; + std::cout.flush(); + } + // Write to log. + log.logStream_ << t; + + return log; + } + +private: + bool isPrintingInCout_; /*!< If flag is set, log is printed in cout and written to the log. */ + + std::stringstream logStream_; /*!< Stream for storing the log. */ +}; diff --git a/odm_meshing/src/OdmMeshing.cpp b/odm_meshing/src/OdmMeshing.cpp new file mode 100644 index 000000000..68bdeaad1 --- /dev/null +++ b/odm_meshing/src/OdmMeshing.cpp @@ -0,0 +1,361 @@ +#include "OdmMeshing.hpp" + + +OdmMeshing::OdmMeshing() : log_(false) +{ + meshCreator_ = pcl::Poisson::Ptr(new pcl::Poisson()); + points_ = pcl::PointCloud::Ptr(new pcl::PointCloud()); + mesh_ = pcl::PolygonMeshPtr(new pcl::PolygonMesh); + decimatedMesh_ = pcl::PolygonMeshPtr(new pcl::PolygonMesh); + + // Set default values + outputFile_ = ""; + logFilePath_ = ""; + + maxVertexCount_ = 0; + treeDepth_ = 0; + + solverDivide_ = 9.0; + samplesPerNode_ = 1.0; + decimationFactor_ = 0.0; + + logFilePath_ = "odm_meshing_log.txt"; + log_ << logFilePath_ << "\n"; +} + +OdmMeshing::~OdmMeshing() +{ + +} + +int OdmMeshing::run(int argc, char **argv) +{ + // If no arguments were passed, print help and return early. + if (argc <= 1) + { + printHelp(); + return EXIT_SUCCESS; + } + + try + { + parseArguments(argc, argv); + + loadPoints(); + + createMesh(); + + decimateMesh(); + + writePlyFile(); + + } + catch (const OdmMeshingException& e) + { + log_.setIsPrintingInCout(true); + log_ << e.what() << "\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (const std::exception& e) + { + log_.setIsPrintingInCout(true); + log_ << "Error in OdmMeshing:\n"; + log_ << e.what() << "\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (...) + { + log_.setIsPrintingInCout(true); + log_ << "Unknwon error in OdmMeshing:\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + + log_.printToFile(logFilePath_); + return EXIT_SUCCESS; +} + + +void OdmMeshing::parseArguments(int argc, char **argv) +{ + + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + // The argument to be parsed. + std::string argument = std::string(argv[argIndex]); + + if(argument == "-help") + { + printHelp(); + } + else if(argument == "-verbose") + { + log_.setIsPrintingInCout(true); + } + else if(argument == "-maxVertexCount" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + std::stringstream ss(argv[argIndex]); + ss >> maxVertexCount_; + if (ss.bad()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value (wrong type)."); + } + log_ << "Vertex count was manually set to: " << maxVertexCount_ << "\n"; + } + else if(argument == "-octreeDepth" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + std::stringstream ss(argv[argIndex]); + ss >> treeDepth_; + if (ss.bad()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value (wrong type)."); + } + log_ << "Octree depth was manually set to: " << treeDepth_ << "\n"; + } + else if(argument == "-solverDivide" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + std::stringstream ss(argv[argIndex]); + ss >> solverDivide_; + if (ss.bad()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value (wrong type)."); + } + log_ << "Numerical solver divisions was manually set to: " << treeDepth_ << "\n"; + } + else if(argument == "-samplesPerNode" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + std::stringstream ss(argv[argIndex]); + ss >> samplesPerNode_; + if (ss.bad()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value (wrong type)."); + } + log_ << "The number of samples per octree node was manually set to: " << samplesPerNode_ << "\n"; + } + else if(argument == "-inputFile" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + inputFile_ = std::string(argv[argIndex]); + std::ifstream testFile(inputFile_.c_str(), std::ios::binary); + if (!testFile.is_open()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value. (file not accessible)"); + } + testFile.close(); + log_ << "Reading point cloud at: " << inputFile_ << "\n"; + } + else if(argument == "-outputFile" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + outputFile_ = std::string(argv[argIndex]); + std::ofstream testFile(outputFile_.c_str()); + if (!testFile.is_open()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value."); + } + testFile.close(); + log_ << "Writing output to: " << outputFile_ << "\n"; + } + else if(argument == "-logFile" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + logFilePath_ = std::string(argv[argIndex]); + std::ofstream testFile(outputFile_.c_str()); + if (!testFile.is_open()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value."); + } + testFile.close(); + log_ << "Writing log information to: " << logFilePath_ << "\n"; + } + else + { + printHelp(); + throw OdmMeshingException("Unrecognised argument '" + argument + "'"); + } + } +} + +void OdmMeshing::loadPoints() +{ + + if(pcl::io::loadPLYFile (inputFile_.c_str(), *points_.get()) == -1) { + throw OdmMeshingException("Error when reading points and normals from:\n" + inputFile_ + "\n"); + } + else + { + log_ << "Successfully loaded " << points_->size() << " points with corresponding normals from file.\n"; + } +} + +void OdmMeshing::printHelp() +{ + bool printInCoutPop = log_.isPrintingInCout(); + log_.setIsPrintingInCout(true); + + log_ << "OpenDroneMapMeshing.exe\n\n"; + + log_ << "Purpose:" << "\n"; + log_ << "Create a mesh from an oriented point cloud (points with normals) using the Poisson surface reconstruction method." << "\n"; + + log_ << "Usage:" << "\n"; + log_ << "The program requires a path to an input PLY point cloud file, all other input parameters are optional." << "\n\n"; + + log_ << "The following flags are available\n"; + log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n"; + log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output stream as well as in the log file.\n\n"; + + log_ << "Parameters are specified as: \"- \", (without <>), and the following parameters are configureable: " << "\n"; + log_ << "\"-inputFile \" (mandatory)" << "\n"; + log_ << "\"Input ascii ply file that must contain a point cloud with normals.\n\n"; + + log_ << "\"-outputFile \" (optional, default: odm_mesh.ply)" << "\n"; + log_ << "\"Target file in which the mesh is saved.\n\n"; + + log_ << "\"-logFile \" (optional, default: odm_meshing_log.txt)" << "\n"; + log_ << "\"Target file in which the mesh is saved.\n\n"; + + log_ << "\"-maxVertexCount \" (optional, default: 100,000)" << "\n"; + log_ << "Desired final vertex count (after decimation), set to 0 to disable decimation.\n\n"; + + log_ << "\"-treeDepth \" (optional, default: 0 (automatic))" << "\n"; + log_ << "Controls octree depth used for poisson reconstruction. Recommended values (9-11).\n" + << "Increasing the value on this parameter will raise initial vertex count." + << "If omitted or zero, the depth is calculated automatically from the input point count.\n\n"; + + log_ << "\"-samplesPerNode \" (optional, default: 1)" << "\n"; + log_ << "Average number of samples (points) per octree node. Increasing this value might help if data is very noisy.\n\n"; + + log_ << "\"-solverDivide \" (optional, default: 9)" << "\n"; + log_ << "Ocree depth at which the Laplacian equation is solved in the surface reconstruction step.\n"; + log_ << "Increasing this value increases computation times slightly but helps reduce memory usage.\n\n"; + + log_.setIsPrintingInCout(printInCoutPop); +} + +void OdmMeshing::createMesh() +{ + + // Attempt to calculate the depth of the tree if unspecified + if (treeDepth_ == 0) + { + treeDepth_ = calcTreeDepth(points_->size()); + } + + log_ << "Octree depth used for reconstruction is: " << treeDepth_ << "\n"; + log_ << "Estimated initial vertex count: " << pow(4, treeDepth_) << "\n\n"; + + meshCreator_->setDepth(treeDepth_); + meshCreator_->setSamplesPerNode(samplesPerNode_); + meshCreator_->setInputCloud(points_); + + // Guarantee manifold mesh. + meshCreator_->setManifold(true); + + // Begin reconstruction + meshCreator_->reconstruct(*mesh_.get()); + + log_ << "Reconstruction complete:\n"; + log_ << "Vertex count: " << mesh_->cloud.width << "\n"; + log_ << "Triangle count: " << mesh_->polygons.size() << "\n\n"; + +} + +void OdmMeshing::decimateMesh() +{ + if (maxVertexCount_ <= 0) + { + log_ << "Vertex count not specified, decimation cancelled.\n"; + return; + } + + if (maxVertexCount_ > mesh_->cloud.height*mesh_->cloud.width) + { + log_ << "Vertex count in mesh lower than initially generated mesh, unable to decimate.\n"; + return; + } + else + { + decimatedMesh_ = pcl::PolygonMeshPtr(new pcl::PolygonMesh); + + double reductionFactor = 1.0 - double(maxVertexCount_)/double(mesh_->cloud.height*mesh_->cloud.width); + + log_ << "Decimating mesh, removing " << reductionFactor*100 << " percent of vertices.\n"; + + pcl::MeshQuadricDecimationVTK decimator; + decimator.setInputMesh(mesh_); + decimator.setTargetReductionFactor(reductionFactor); + decimator.process(*decimatedMesh_.get()); + + log_ << "Decimation complete.\n"; + log_ << "Decimated vertex count: " << decimatedMesh_->cloud.width << "\n"; + log_ << "Decimated triangle count: " << decimatedMesh_->polygons.size() << "\n\n"; + + mesh_ = decimatedMesh_; + } +} + +int OdmMeshing::calcTreeDepth(size_t nPoints) +{ + // Assume points are located (roughly) in a plane. + double squareSide = std::sqrt(double(nPoints)); + + // Calculate octree depth such that if points were equally distributed in + // a quadratic plane, there would be at least 1 point per octree node. + int depth = 0; + while(std::pow(2,depth) < squareSide/2) + { + depth++; + } + return depth; +} + +void OdmMeshing::writePlyFile() +{ + log_ << "Saving mesh to file.\n"; + if (pcl::io::savePLYFile(outputFile_.c_str(), *mesh_.get()) == -1) { + throw OdmMeshingException("Error when saving mesh to file:\n" + outputFile_ + "\n"); + } + else + { + log_ << "Successfully wrote mesh to:\n" + << outputFile_ << "\n"; + } +} diff --git a/odm_meshing/src/OdmMeshing.cpp~ b/odm_meshing/src/OdmMeshing.cpp~ new file mode 100644 index 000000000..68bdeaad1 --- /dev/null +++ b/odm_meshing/src/OdmMeshing.cpp~ @@ -0,0 +1,361 @@ +#include "OdmMeshing.hpp" + + +OdmMeshing::OdmMeshing() : log_(false) +{ + meshCreator_ = pcl::Poisson::Ptr(new pcl::Poisson()); + points_ = pcl::PointCloud::Ptr(new pcl::PointCloud()); + mesh_ = pcl::PolygonMeshPtr(new pcl::PolygonMesh); + decimatedMesh_ = pcl::PolygonMeshPtr(new pcl::PolygonMesh); + + // Set default values + outputFile_ = ""; + logFilePath_ = ""; + + maxVertexCount_ = 0; + treeDepth_ = 0; + + solverDivide_ = 9.0; + samplesPerNode_ = 1.0; + decimationFactor_ = 0.0; + + logFilePath_ = "odm_meshing_log.txt"; + log_ << logFilePath_ << "\n"; +} + +OdmMeshing::~OdmMeshing() +{ + +} + +int OdmMeshing::run(int argc, char **argv) +{ + // If no arguments were passed, print help and return early. + if (argc <= 1) + { + printHelp(); + return EXIT_SUCCESS; + } + + try + { + parseArguments(argc, argv); + + loadPoints(); + + createMesh(); + + decimateMesh(); + + writePlyFile(); + + } + catch (const OdmMeshingException& e) + { + log_.setIsPrintingInCout(true); + log_ << e.what() << "\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (const std::exception& e) + { + log_.setIsPrintingInCout(true); + log_ << "Error in OdmMeshing:\n"; + log_ << e.what() << "\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (...) + { + log_.setIsPrintingInCout(true); + log_ << "Unknwon error in OdmMeshing:\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + + log_.printToFile(logFilePath_); + return EXIT_SUCCESS; +} + + +void OdmMeshing::parseArguments(int argc, char **argv) +{ + + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + // The argument to be parsed. + std::string argument = std::string(argv[argIndex]); + + if(argument == "-help") + { + printHelp(); + } + else if(argument == "-verbose") + { + log_.setIsPrintingInCout(true); + } + else if(argument == "-maxVertexCount" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + std::stringstream ss(argv[argIndex]); + ss >> maxVertexCount_; + if (ss.bad()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value (wrong type)."); + } + log_ << "Vertex count was manually set to: " << maxVertexCount_ << "\n"; + } + else if(argument == "-octreeDepth" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + std::stringstream ss(argv[argIndex]); + ss >> treeDepth_; + if (ss.bad()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value (wrong type)."); + } + log_ << "Octree depth was manually set to: " << treeDepth_ << "\n"; + } + else if(argument == "-solverDivide" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + std::stringstream ss(argv[argIndex]); + ss >> solverDivide_; + if (ss.bad()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value (wrong type)."); + } + log_ << "Numerical solver divisions was manually set to: " << treeDepth_ << "\n"; + } + else if(argument == "-samplesPerNode" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + std::stringstream ss(argv[argIndex]); + ss >> samplesPerNode_; + if (ss.bad()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value (wrong type)."); + } + log_ << "The number of samples per octree node was manually set to: " << samplesPerNode_ << "\n"; + } + else if(argument == "-inputFile" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + inputFile_ = std::string(argv[argIndex]); + std::ifstream testFile(inputFile_.c_str(), std::ios::binary); + if (!testFile.is_open()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value. (file not accessible)"); + } + testFile.close(); + log_ << "Reading point cloud at: " << inputFile_ << "\n"; + } + else if(argument == "-outputFile" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + outputFile_ = std::string(argv[argIndex]); + std::ofstream testFile(outputFile_.c_str()); + if (!testFile.is_open()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value."); + } + testFile.close(); + log_ << "Writing output to: " << outputFile_ << "\n"; + } + else if(argument == "-logFile" && argIndex < argc) + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmMeshingException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + logFilePath_ = std::string(argv[argIndex]); + std::ofstream testFile(outputFile_.c_str()); + if (!testFile.is_open()) + { + throw OdmMeshingException("Argument '" + argument + "' has a bad value."); + } + testFile.close(); + log_ << "Writing log information to: " << logFilePath_ << "\n"; + } + else + { + printHelp(); + throw OdmMeshingException("Unrecognised argument '" + argument + "'"); + } + } +} + +void OdmMeshing::loadPoints() +{ + + if(pcl::io::loadPLYFile (inputFile_.c_str(), *points_.get()) == -1) { + throw OdmMeshingException("Error when reading points and normals from:\n" + inputFile_ + "\n"); + } + else + { + log_ << "Successfully loaded " << points_->size() << " points with corresponding normals from file.\n"; + } +} + +void OdmMeshing::printHelp() +{ + bool printInCoutPop = log_.isPrintingInCout(); + log_.setIsPrintingInCout(true); + + log_ << "OpenDroneMapMeshing.exe\n\n"; + + log_ << "Purpose:" << "\n"; + log_ << "Create a mesh from an oriented point cloud (points with normals) using the Poisson surface reconstruction method." << "\n"; + + log_ << "Usage:" << "\n"; + log_ << "The program requires a path to an input PLY point cloud file, all other input parameters are optional." << "\n\n"; + + log_ << "The following flags are available\n"; + log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n"; + log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output stream as well as in the log file.\n\n"; + + log_ << "Parameters are specified as: \"- \", (without <>), and the following parameters are configureable: " << "\n"; + log_ << "\"-inputFile \" (mandatory)" << "\n"; + log_ << "\"Input ascii ply file that must contain a point cloud with normals.\n\n"; + + log_ << "\"-outputFile \" (optional, default: odm_mesh.ply)" << "\n"; + log_ << "\"Target file in which the mesh is saved.\n\n"; + + log_ << "\"-logFile \" (optional, default: odm_meshing_log.txt)" << "\n"; + log_ << "\"Target file in which the mesh is saved.\n\n"; + + log_ << "\"-maxVertexCount \" (optional, default: 100,000)" << "\n"; + log_ << "Desired final vertex count (after decimation), set to 0 to disable decimation.\n\n"; + + log_ << "\"-treeDepth \" (optional, default: 0 (automatic))" << "\n"; + log_ << "Controls octree depth used for poisson reconstruction. Recommended values (9-11).\n" + << "Increasing the value on this parameter will raise initial vertex count." + << "If omitted or zero, the depth is calculated automatically from the input point count.\n\n"; + + log_ << "\"-samplesPerNode \" (optional, default: 1)" << "\n"; + log_ << "Average number of samples (points) per octree node. Increasing this value might help if data is very noisy.\n\n"; + + log_ << "\"-solverDivide \" (optional, default: 9)" << "\n"; + log_ << "Ocree depth at which the Laplacian equation is solved in the surface reconstruction step.\n"; + log_ << "Increasing this value increases computation times slightly but helps reduce memory usage.\n\n"; + + log_.setIsPrintingInCout(printInCoutPop); +} + +void OdmMeshing::createMesh() +{ + + // Attempt to calculate the depth of the tree if unspecified + if (treeDepth_ == 0) + { + treeDepth_ = calcTreeDepth(points_->size()); + } + + log_ << "Octree depth used for reconstruction is: " << treeDepth_ << "\n"; + log_ << "Estimated initial vertex count: " << pow(4, treeDepth_) << "\n\n"; + + meshCreator_->setDepth(treeDepth_); + meshCreator_->setSamplesPerNode(samplesPerNode_); + meshCreator_->setInputCloud(points_); + + // Guarantee manifold mesh. + meshCreator_->setManifold(true); + + // Begin reconstruction + meshCreator_->reconstruct(*mesh_.get()); + + log_ << "Reconstruction complete:\n"; + log_ << "Vertex count: " << mesh_->cloud.width << "\n"; + log_ << "Triangle count: " << mesh_->polygons.size() << "\n\n"; + +} + +void OdmMeshing::decimateMesh() +{ + if (maxVertexCount_ <= 0) + { + log_ << "Vertex count not specified, decimation cancelled.\n"; + return; + } + + if (maxVertexCount_ > mesh_->cloud.height*mesh_->cloud.width) + { + log_ << "Vertex count in mesh lower than initially generated mesh, unable to decimate.\n"; + return; + } + else + { + decimatedMesh_ = pcl::PolygonMeshPtr(new pcl::PolygonMesh); + + double reductionFactor = 1.0 - double(maxVertexCount_)/double(mesh_->cloud.height*mesh_->cloud.width); + + log_ << "Decimating mesh, removing " << reductionFactor*100 << " percent of vertices.\n"; + + pcl::MeshQuadricDecimationVTK decimator; + decimator.setInputMesh(mesh_); + decimator.setTargetReductionFactor(reductionFactor); + decimator.process(*decimatedMesh_.get()); + + log_ << "Decimation complete.\n"; + log_ << "Decimated vertex count: " << decimatedMesh_->cloud.width << "\n"; + log_ << "Decimated triangle count: " << decimatedMesh_->polygons.size() << "\n\n"; + + mesh_ = decimatedMesh_; + } +} + +int OdmMeshing::calcTreeDepth(size_t nPoints) +{ + // Assume points are located (roughly) in a plane. + double squareSide = std::sqrt(double(nPoints)); + + // Calculate octree depth such that if points were equally distributed in + // a quadratic plane, there would be at least 1 point per octree node. + int depth = 0; + while(std::pow(2,depth) < squareSide/2) + { + depth++; + } + return depth; +} + +void OdmMeshing::writePlyFile() +{ + log_ << "Saving mesh to file.\n"; + if (pcl::io::savePLYFile(outputFile_.c_str(), *mesh_.get()) == -1) { + throw OdmMeshingException("Error when saving mesh to file:\n" + outputFile_ + "\n"); + } + else + { + log_ << "Successfully wrote mesh to:\n" + << outputFile_ << "\n"; + } +} diff --git a/odm_meshing/src/OdmMeshing.hpp b/odm_meshing/src/OdmMeshing.hpp new file mode 100644 index 000000000..8e5599894 --- /dev/null +++ b/odm_meshing/src/OdmMeshing.hpp @@ -0,0 +1,117 @@ +#pragma once + +// STL +#include +#include + +// PCL +#include +#include +#include + +// Logging +#include "Logger.hpp" + +/*! + * \brief The OdmMeshing class is used to create a triangulated mesh using the Poisson method. + * The class reads an oriented point cloud (coordinates and normals) from a PLY ascii + * file and outputs the resulting welded manifold mesh on the form of an ASCII PLY-file. + * The class uses file read and write functions from pcl. + */ +class OdmMeshing +{ +public: + OdmMeshing(); + ~OdmMeshing(); + + /*! + * \brief run Runs the meshing functionality using the provided input arguments. + * For a list of accepted arguments, please see the main page documentation or + * call the program with parameter "-help". + * \param argc Application argument count. + * \param argv Argument values. + * \return 0 If successful. + */ + int run(int argc, char **argv); + +private: + + /*! + * \brief parseArguments Parses command line arguments. + * \param argc Application argument count. + * \param argv Argument values. + */ + void parseArguments(int argc, char** argv); + + /*! + * \brief createMesh Sets up the pcl::Poisson meshing class using provided arguments and calls + * it to start the meshing. + */ + void createMesh(); + + /*! + * \brief loadPoints Loads a PLY ascii file with points and normals from file. + */ + void loadPoints(); + + /*! + * \brief decimateMesh Performs post-processing on the form of quadric decimation to generate a mesh + * that has a higher density in areas with a lot of structure. + */ + void decimateMesh(); + + /*! + * \brief writePlyFile Writes the mesh to file on the Ply format. + */ + void writePlyFile(); + + /*! + * \brief printHelp Prints help, explaining usage. Can be shown by calling the program with argument: "-help". + */ + void printHelp(); + + /*! + * \brief calcTreeDepth Attepts to calculate the depth of the tree using the point cloud. + * The function makes the assumption points are located roughly in a plane + * (fairly reasonable for ortho-terrain photos) and tries to generate a mesh using + * an octree with an appropriate resolution. + * \param nPoints The total number of points in the input point cloud. + * \return The calcualated octree depth. + */ + int calcTreeDepth(size_t nPoints); + + Logger log_; /**< Logging object. */ + + pcl::Poisson::Ptr meshCreator_; /**< PCL poisson meshing class. */ + + pcl::PointCloud::Ptr points_; /**< Input point and normals. */ + pcl::PolygonMeshPtr mesh_; /**< PCL polygon mesh. */ + pcl::PolygonMeshPtr decimatedMesh_; /**< Decimated polygon mesh. */ + + std::string inputFile_; /**< Path to a file containing points and normals. */ + std::string outputFile_; /**< Path to the destination file. */ + std::string logFilePath_; /**< Path to the log file. */ + + unsigned int maxVertexCount_; /**< Desired output vertex count. */ + unsigned int treeDepth_; /**< Depth of octree used for reconstruction. */ + + double samplesPerNode_; /**< Samples per octree node.*/ + double solverDivide_; /**< Depth at which the Laplacian equation solver is run during surface estimation.*/ + double decimationFactor_; /**< Percentage of points to remove when decimating the mesh. */ +}; + +/*! + * \brief The OdmMeshingException class + */ +class OdmMeshingException : public std::exception +{ + +public: + OdmMeshingException() : message("Error in OdmMeshing") {} + OdmMeshingException(std::string msgInit) : message("Error in OdmMeshing:\n" + msgInit) {} + ~OdmMeshingException() throw() {} + virtual const char* what() const throw() {return message.c_str(); } + +private: + std::string message; /**< The error message **/ +}; diff --git a/odm_meshing/src/main.cpp b/odm_meshing/src/main.cpp new file mode 100644 index 000000000..f99f20ce7 --- /dev/null +++ b/odm_meshing/src/main.cpp @@ -0,0 +1,20 @@ +// Insert license here. + +// Include meshing source code. +#include "OdmMeshing.hpp" + +/*! + * \mainpage main OpenDroneMap Meshing Module + * + * The OpenDroneMap Meshing Module generates a welded, manifold mesh using the Poisson + * surface reconstruction algorithm from any oriented point cloud (points with corresponding normals). + * + */ + +int main(int argc, char** argv) +{ + + OdmMeshing meshCreator; + return meshCreator.run(argc, argv); + +} diff --git a/odm_orthophoto/CMakeLists.txt b/odm_orthophoto/CMakeLists.txt new file mode 100644 index 000000000..2b83592dc --- /dev/null +++ b/odm_orthophoto/CMakeLists.txt @@ -0,0 +1,33 @@ +project(odm_orthophoto) +cmake_minimum_required(VERSION 2.8) + +# Set pcl dir to the input spedified with option -DPCL_DIR="path" +set(PCL_DIR "PCL_DIR-NOTFOUND" CACHE "PCL_DIR" "Path to the pcl installation directory") + +# Add compiler options. +add_definitions(-Wall -Wextra) + +# Find pcl at the location specified by PCL_DIR +find_package(PCL 1.7 HINTS "${PCL_DIR}/share/pcl-1.7" REQUIRED) + +# Find OpenCV at the default location +find_package(OpenCV REQUIRED) + +# Only link with required opencv modules. +set(OpenCV_LIBS opencv_core opencv_imgproc opencv_highgui) + +# Add the PCL, Eigen and OpenCV include dirs. +# Necessary since the PCL_INCLUDE_DIR variable set by find_package is broken.) +include_directories(${PCL_ROOT}/include/pcl-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}) +include_directories(${EIGEN_ROOT}) +include_directories(${OpenCV_INCLUDE_DIRS}) + +#library_directories(${OpenCV_LIBRARY_DIRS}) + +# Add source directory +aux_source_directory("./src" SRC_LIST) + +# Add exectuteable +add_executable(${PROJECT_NAME} ${SRC_LIST}) +target_link_libraries(odm_orthophoto ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_SURFACE_LIBRARIES} ${OpenCV_LIBS}) + diff --git a/odm_orthophoto/src/Logger.cpp b/odm_orthophoto/src/Logger.cpp new file mode 100644 index 000000000..46b96fd72 --- /dev/null +++ b/odm_orthophoto/src/Logger.cpp @@ -0,0 +1,31 @@ +#include "Logger.hpp" + + +Logger::Logger(bool isPrintingInCout) : isPrintingInCout_(isPrintingInCout) +{ + +} + +Logger::~Logger() +{ + +} + +void Logger::print(std::string filePath) +{ + std::ofstream file(filePath.c_str(), std::ios::binary); + file << logStream_.str(); + file.close(); +} + +bool Logger::isPrintingInCout() const +{ + return isPrintingInCout_; +} + +void Logger::setIsPrintingInCout(bool isPrintingInCout) +{ + isPrintingInCout_ = isPrintingInCout; +} + + diff --git a/odm_orthophoto/src/Logger.hpp b/odm_orthophoto/src/Logger.hpp new file mode 100644 index 000000000..61520146e --- /dev/null +++ b/odm_orthophoto/src/Logger.hpp @@ -0,0 +1,68 @@ +#pragma once + +// STL +#include +#include +#include +#include + +/*! + * \brief The Logger class is used to store program messages in a log file. + * \details By using the << operator while printInCout is set, the class writes both to + * cout and to file, if the flag is not set, output is written to file only. + */ +class Logger +{ +public: + /*! + * \brief Logger Contains functionality for printing and displaying log information. + * \param printInCout Flag toggling if operator << also writes to cout. + */ + Logger(bool isPrintingInCout = true); + + /*! + * \brief Destructor. + */ + ~Logger(); + + /*! + * \brief print Prints the contents of the log to file. + * \param filePath Path specifying where to write the log. + */ + void print(std::string filePath); + + /*! + * \brief isPrintingInCout Check if console printing flag is set. + * \return Console printing flag. + */ + bool isPrintingInCout() const; + + /*! + * \brief setIsPrintingInCout Set console printing flag. + * \param isPrintingInCout Value, if true, messages added to the log are also printed in cout. + */ + void setIsPrintingInCout(bool isPrintingInCout); + + /*! + * Operator for printing messages to log and in the standard output stream if desired. + */ + template + friend Logger& operator<< (Logger &log, T t) + { + // If console printing is enabled. + if (log.isPrintingInCout_) + { + std::cout << t; + std::cout.flush(); + } + // Write to log. + log.logStream_ << t; + + return log; + } + +private: + bool isPrintingInCout_; /*!< If flag is set, log is printed in cout and written to the log. */ + + std::stringstream logStream_; /*!< Stream for storing the log. */ +}; diff --git a/odm_orthophoto/src/OdmOrthoPhoto.cpp b/odm_orthophoto/src/OdmOrthoPhoto.cpp new file mode 100644 index 000000000..dd43db4c7 --- /dev/null +++ b/odm_orthophoto/src/OdmOrthoPhoto.cpp @@ -0,0 +1,646 @@ + + +// This +#include "OdmOrthoPhoto.hpp" + +OdmOrthoPhoto::OdmOrthoPhoto() + :log_(false) +{ + inputFile_ = ""; + outputFile_ = "ortho.jpg"; + logFile_ = "log.txt"; + + resolution_ = 0.0f; + boundryPoint1_[0] = 0.0f; boundryPoint1_[1] = 0.0f; + boundryPoint2_[0] = 0.0f; boundryPoint2_[1] = 0.0f; + boundryPoint3_[0] = 0.0f; boundryPoint3_[1] = 0.0f; + boundryPoint4_[0] = 0.0f; boundryPoint4_[1] = 0.0f; +} + +OdmOrthoPhoto::~OdmOrthoPhoto() +{ +} + +int OdmOrthoPhoto::run(int argc, char *argv[]) +{ + try + { + parseArguments(argc, argv); + createOrthoPhoto(); + } + catch (const OdmOrthoPhotoException& e) + { + log_ << e.what() << "\n"; + log_.print(logFile_); + return EXIT_FAILURE; + } + catch (const std::exception& e) + { + log_ << "Error in OdmOrthoPhoto:\n"; + log_ << e.what() << "\n"; + log_.print(logFile_); + return EXIT_FAILURE; + } + catch (...) + { + log_ << "Unknown error, terminating:\n"; + log_.print(logFile_); + return EXIT_FAILURE; + } + + log_.print(logFile_); + + return EXIT_SUCCESS; +} + +void OdmOrthoPhoto::parseArguments(int argc, char *argv[]) +{ + logFile_ = std::string(argv[0]) + "_log.txt"; + log_ << logFile_ << "\n"; + + // If no arguments were passed, print help. + if (argc == 1) + { + printHelp(); + } + + log_ << "Arguments given\n"; + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + log_ << argv[argIndex] << '\n'; + } + + log_ << '\n'; + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + // The argument to be parsed. + std::string argument = std::string(argv[argIndex]); + + if(argument == "-help") + { + printHelp(); + } + else if(argument == "-resolution") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + std::stringstream ss(argv[argIndex]); + ss >> resolution_; + log_ << "Resolution count was set to: " << resolution_ << "pixels/meter\n"; + } + else if(argument == "-boundry") + { + if(argIndex+8 >= argc) + { + throw OdmOrthoPhotoException("Argument '" + argument + "' expects 8 more input following it, but no more inputs were provided."); + } + + std::stringstream ss; + ss << argv[argIndex+1] << " " << argv[argIndex+2] << " " << argv[argIndex+3] << " " << argv[argIndex+4] << " " << argv[argIndex+5] << " " << argv[argIndex+6] << " " << argv[argIndex+7] << " " << argv[argIndex+8]; + ss >> boundryPoint1_[0] >> boundryPoint1_[1] >> boundryPoint2_[0] >> boundryPoint2_[1] >> boundryPoint3_[0] >> boundryPoint3_[1] >> boundryPoint4_[0] >> boundryPoint4_[1]; + argIndex += 8; + + log_ << "Boundry point 1 was set to: " << boundryPoint1_[0] << ", " << boundryPoint1_[1] << '\n'; + log_ << "Boundry point 2 was set to: " << boundryPoint2_[0] << ", " << boundryPoint2_[1] << '\n'; + log_ << "Boundry point 3 was set to: " << boundryPoint3_[0] << ", " << boundryPoint3_[1] << '\n'; + log_ << "Boundry point 4 was set to: " << boundryPoint4_[0] << ", " << boundryPoint4_[1] << '\n'; + } + else if(argument == "-verbose") + { + log_.setIsPrintingInCout(true); + } + else if(argument == "-inputFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + inputFile_ = std::string(argv[argIndex]); + log_ << "Reading textured mesh from: " << inputFile_ << "\n"; + } + else if(argument == "-outputFile" && argIndex < argc) + { + argIndex++; + if (argIndex >= argc) + { + throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided."); + } + outputFile_ = std::string(argv[argIndex]); + log_ << "Writing output to: " << outputFile_ << "\n"; + } + else + { + printHelp(); + throw OdmOrthoPhotoException("Unrecognised argument '" + argument + "'"); + } + } +} + +void OdmOrthoPhoto::printHelp() +{ + log_.setIsPrintingInCout(true); + + log_ << "OpenDroneMapOrthoPhoto.exe\n\n"; + + log_ << "Purpose:" << "\n"; + log_ << "Create an orthograpical photo from an oriented textured mesh." << "\n"; + + log_ << "Usage:" << "\n"; + log_ << "The program requires a path to an input OBJ mesh file, resolution and boundry points to define the area. All other input parameters are optional." << "\n\n"; + + log_ << "The following flags are available\n"; + log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n"; + log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output stream as well as in the log file.\n\n"; + + log_ << "Parameters are specified as: \"- \", (without <>), and the following parameters are configureable: " << "\n"; + log_ << "\"-inputFile \" (mandatory)" << "\n"; + log_ << "\"Input obj file that must contain a textured mesh.\n\n"; + + log_ << "\"-outputFile \" (optional, default: ortho.jpg)" << "\n"; + log_ << "\"Target file in which the orthophoto is saved.\n\n"; + + log_ << "\"-resolution \" (mandatory)" << "\n"; + log_ << "\"The number of pixels used per meter.\n\n"; + + log_ << "\"-boundry \" (mandatory)" << "\n"; + log_ << "\"Describes the area which should be covered in the ortho photo. The area will be a bounding box containing all four points.\n\n"; + + log_.setIsPrintingInCout(false); +} + +void OdmOrthoPhoto::createOrthoPhoto() +{ + if(inputFile_.empty()) + { + throw OdmOrthoPhotoException("Failed to create ortho photo, no texture mesh given."); + return; + } + + log_ << '\n'; + log_ << "Reading mesh file...\n"; + // The textureds mesh. + pcl::TextureMesh mesh; + pcl::io::loadOBJFile(inputFile_, mesh); + log_ << ".. mesh file read.\n"; + + // Does the model have more than one material? + multiMaterial_ = 1 < mesh.tex_materials.size(); + + if(multiMaterial_) + { + // Need to check relationship between texture coordinates and faces. + if(!isModelOk(mesh)) + { + throw OdmOrthoPhotoException("Could not generate ortho photo: The given mesh has multiple textures, but the number of texture coordinates is NOT equal to 3 times the number of faces."); + } + } + + // The minimum and maximum boundry values. + float xMax, xMin, yMax, yMin; + xMin = std::min(std::min(boundryPoint1_[0], boundryPoint2_[0]), std::min(boundryPoint3_[0], boundryPoint4_[0])); + xMax = std::max(std::max(boundryPoint1_[0], boundryPoint2_[0]), std::max(boundryPoint3_[0], boundryPoint4_[0])); + yMin = std::min(std::min(boundryPoint1_[1], boundryPoint2_[1]), std::min(boundryPoint3_[1], boundryPoint4_[1])); + yMax = std::max(std::max(boundryPoint1_[1], boundryPoint2_[1]), std::max(boundryPoint3_[1], boundryPoint4_[1])); + + log_ << "Ortho photo area x : " << xMin << " -> " << xMax << '\n'; + log_ << "Ortho photo area y : " << yMin << " -> " << yMax << '\n'; + + // The size of the area. + float xDiff = xMax - xMin; + float yDiff = yMax - yMin; + + // The resolution neccesary to fit the area with the given resolution. + int rowRes = static_cast(std::ceil(resolution_*xDiff)); + int colRes = static_cast(std::ceil(resolution_*yDiff)); + log_ << "Ortho photo resolution, width x height : " << colRes << "x" << rowRes << '\n'; + + if(0 >= rowRes*colRes) + { + if(0 >= rowRes) + { + log_ << "Warning: ortho photo has zero area, height = " << rowRes << ". Forcing height = 1.\n"; + rowRes = 1; + } + if(0 >= colRes) + { + log_ << "Warning: ortho photo has zero area, width = " << colRes << ". Forcing width = 1.\n"; + colRes = 1; + } + log_ << "New ortho photo resolution, width x height : " << colRes << "x" << rowRes << '\n'; + } + + // Init ortho photo + photo_ = cv::Mat::zeros(rowRes, colRes, CV_8UC3) + cv::Scalar(255, 255, 255); + depth_ = cv::Mat::zeros(rowRes, colRes, CV_32F) - std::numeric_limits::infinity(); + + // Contains the vertices of the mesh. + pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (mesh.cloud, *meshCloud); + + // Creates a transformation which aligns the area for the ortho photo. + Eigen::Transform transform = getROITransform(xMin, -yMax); + + log_ << "Translating and scaling mesh...\n"; + + // Move the mesh into position. + pcl::transformPointCloud(*meshCloud, *meshCloud, transform); + log_ << ".. mesh translated and scaled.\n"; + + // Flatten texture coordiantes. + std::vector uvs; + for(size_t t = 0; t < mesh.tex_coordinates.size(); ++t) + { + uvs.insert(uvs.end(), mesh.tex_coordinates[t].begin(), mesh.tex_coordinates[t].end()); + } + + // The current material texture + cv::Mat texture; + + // Used to keep track of the global face index. + size_t faceOff = 0; + + log_ << '\n'; + log_ << "Rendering the ortho photo...\n"; + + // Iterate over each part of the mesh (one per material). + for(size_t t = 0; t < mesh.tex_materials.size(); ++t) + { + // The material of the current submesh. + pcl::TexMaterial material = mesh.tex_materials[t]; + texture = cv::imread(material.tex_file); + + // Check for missing files. + if(texture.empty()) + { + log_ << "Material texture could not be read:\n"; + log_ << material.tex_file << '\n'; + log_ << "Could not be read as image, does the file exist?\n"; + continue; // Skip to next material. + } + + // The faces of the current submesh. + std::vector faces = mesh.tex_polygons[t]; + + // Iterate over each face... + for(size_t faceIndex = 0; faceIndex < faces.size(); ++faceIndex) + { + // The current polygon. + pcl::Vertices polygon = faces[faceIndex]; + + // ... and draw it into the ortho photo. + drawTexturedTriangle(texture, polygon, meshCloud, uvs, faceIndex+faceOff); + } + faceOff += faces.size(); + log_ << "Material " << t << " rendered.\n"; + } + log_ << "...ortho photo rendered\n"; + + log_ << '\n'; + log_ << "Writing ortho photo to " << outputFile_ << "\n"; + cv::imwrite(outputFile_, photo_); + log_ << "Orthophoto generation done.\n"; +} + +Eigen::Transform OdmOrthoPhoto::getROITransform(float xMin, float yMin) const +{ + // The tranform used to move the chosen area into the ortho photo. + Eigen::Transform transform; + + transform(0, 0) = resolution_; + transform(1, 0) = 0.0f; + transform(2, 0) = 0.0f; + transform(3, 0) = 0.0f; + + transform(0, 1) = 0.0f; + transform(1, 1) = -resolution_; + transform(2, 1) = 0.0f; + transform(3, 1) = 0.0f; + + transform(0, 2) = 0.0f; + transform(1, 2) = 0.0f; + transform(2, 2) = 1.0f; + transform(3, 2) = 0.0f; + + transform(0, 3) = -xMin*resolution_; + transform(1, 3) = -yMin*resolution_; + transform(2, 3) = 0.0f; + transform(3, 3) = 1.0f; + + return transform; +} + +void OdmOrthoPhoto::drawTexturedTriangle(const cv::Mat &texture, const pcl::Vertices &polygon, const pcl::PointCloud::Ptr &meshCloud, const std::vector &uvs, size_t faceIndex) +{ + // The index to the vertices of the polygon. + size_t v1i = polygon.vertices[0]; + size_t v2i = polygon.vertices[1]; + size_t v3i = polygon.vertices[2]; + + // The polygon's points. + pcl::PointXYZ v1 = meshCloud->points[v1i]; + pcl::PointXYZ v2 = meshCloud->points[v2i]; + pcl::PointXYZ v3 = meshCloud->points[v3i]; + + if(isSliverPolygon(v1, v2, v3)) + { + log_ << "Warning: Sliver polygon found at face index " << faceIndex << '\n'; + return; + } + + // The face data. Position v*{x,y,z}. Texture coordinate v*{u,v}. * is the vertex number in the polygon. + float v1x, v1y, v1z, v1u, v1v; + float v2x, v2y, v2z, v2u, v2v; + float v3x, v3y, v3z, v3u, v3v; + + // Barycentric coordinates of the currently rendered point. + float l1, l2, l3; + + // The size of the photo, as float. + float fRows, fCols; + fRows = static_cast(texture.rows); + fCols = static_cast(texture.cols); + + // Get vertex position. + v1x = v1.x; v1y = v1.y; v1z = v1.z; + v2x = v2.x; v2y = v2.y; v2z = v2.z; + v3x = v3.x; v3y = v3.y; v3z = v3.z; + + // Get texture coorinates. + if(multiMaterial_) + { + v1u = uvs[3*faceIndex][0]; v1v = uvs[3*faceIndex][1]; + v2u = uvs[3*faceIndex+1][0]; v2v = uvs[3*faceIndex+1][1]; + v3u = uvs[3*faceIndex+2][0]; v3v = uvs[3*faceIndex+2][1]; + } + else + { + v1u = uvs[v1i][0]; v1v = uvs[v1i][1]; + v2u = uvs[v2i][0]; v2v = uvs[v2i][1]; + v3u = uvs[v3i][0]; v3v = uvs[v3i][1]; + } + + // Check bounding box overlap. + int xMin = static_cast(std::min(std::min(v1x, v2x), v3x)); + if(xMin > photo_.cols) + { + return; // Outside to the right. + } + int xMax = static_cast(std::max(std::max(v1x, v2x), v3x)); + if(xMax < 0) + { + return; // Outside to the left. + } + int yMin = static_cast(std::min(std::min(v1y, v2y), v3y)); + if(yMin > photo_.rows) + { + return; // Outside to the top. + } + int yMax = static_cast(std::max(std::max(v1y, v2y), v3y)); + if(yMax < 0) + { + return; // Outside to the bottom. + } + + // Top point row and column positions + float topR, topC; + // Middle point row and column positions + float midR, midC; + // Bottom point row and column positions + float botR, botC; + + // Find top, middle and bottom points. + if(v1y < v2y) + { + if(v1y < v3y) + { + if(v2y < v3y) + { + // 1 -> 2 -> 3 + topR = v1y; topC = v1x; + midR = v2y; midC = v2x; + botR = v3y; botC = v3x; + } + else + { + // 1 -> 3 -> 2 + topR = v1y; topC = v1x; + midR = v3y; midC = v3x; + botR = v2y; botC = v2x; + } + } + else + { + // 3 -> 1 -> 2 + topR = v3y; topC = v3x; + midR = v1y; midC = v1x; + botR = v2y; botC = v2x; + } + } + else // v2y <= v1y + { + if(v2y < v3y) + { + if(v1y < v3y) + { + // 2 -> 1 -> 3 + topR = v2y; topC = v2x; + midR = v1y; midC = v1x; + botR = v3y; botC = v3x; + } + else + { + // 2 -> 3 -> 1 + topR = v2y; topC = v2x; + midR = v3y; midC = v3x; + botR = v1y; botC = v1x; + } + } + else + { + // 3 -> 2 -> 1 + topR = v3y; topC = v3x; + midR = v2y; midC = v2x; + botR = v1y; botC = v1x; + } + } + + // The step along column for every step along r. Top to middle. + float ctmdr; + // The step along column for every step along r. Top to bottom. + float ctbdr; + // The step along column for every step along r. Middle to bottom. + float cmbdr; + + ctbdr = (botC-topC)/(botR-topR); + + // The current column position, from top to middle. + float ctm = topC; + // The current column position, from top to bottom. + float ctb = topC; + + // Check for vertical line between middle and top. + if(FLT_EPSILON < midR-topR) + { + ctmdr = (midC-topC)/(midR-topR); + + // Describes the offset from the pixel coordiante to the triangle point. + float rRoundOff = topR - std::floor(topR); + + // Travers along row from top to middle. + for(int rq = static_cast(std::floor(topR))+1; rq <= static_cast(std::floor(midR)); ++rq) + { + // Set the current column positions. + ctm = topC + ctmdr*(static_cast(rq)-topR); + ctb = topC + ctbdr*(static_cast(rq)-topR); + + // Describes the offset from the pixel coordiante to the triangle point. + float cRoundOff = std::min(ctm, ctb) - std::floor(std::min(ctm, ctb)); + + for(int cq = static_cast(std::floor(std::min(ctm, ctb))); cq < static_cast(std::floor(std::max(ctm, ctb))); ++cq) + { + if(0 <= rq && rq < photo_.rows && 0 <= cq && cq < photo_.cols) + { + // Get barycentric coordinates for the current point. + getBarycentricCoordiantes(v1, v2, v3, static_cast(cq)+cRoundOff, static_cast(rq)+rRoundOff, l1, l2, l3); + + // The z value for the point. + float z = v1z*l1+v2z*l2+v3z*l3; + + // Check depth + float depthValue = depth_.at(rq, cq); + if(z < depthValue) + { + // Current is behind last, don't draw. + continue; + } + + // The uv values of the point. + float u, v; + u = v1u*l1+v2u*l2+v3u*l3; + v = v1v*l1+v2v*l2+v3v*l3; + + // Nearest neighbour texture interpolation. + int uq = static_cast(u*fCols); + int vq = static_cast((1.0f-v)*fRows); + photo_.at(rq,cq) = texture.at(vq, uq); + + // Update depth buffer. + depth_.at(rq, cq) = z; + } + } + } + } + + if(FLT_EPSILON < botR-midR) + { + cmbdr = (botC-midC)/(botR-midR); + + // The current column position, from middle to bottom. + float cmb = midC; + + // Describes the offset from the pixel coordiante to the triangle point. + float rRoundOff = midR - std::floor(midR); + + // Travers along row from middle to bottom. + for(int rq = static_cast(std::floor(midR))+1; rq <= static_cast(std::floor(botR)); ++rq) + { + // Set the current column positions. + ctb = topC + ctbdr*(static_cast(rq)-topR); + cmb = midC + cmbdr*(static_cast(rq)-midR); + + // Describes the offset from the pixel coordiante to the triangle point. + float cRoundOff = std::min(cmb, ctb) - std::floor(std::min(cmb, ctb)); + + for(int cq = static_cast(std::floor(std::min(cmb, ctb))); cq < static_cast(std::floor(std::max(cmb, ctb))); ++cq) + { + if(0 <= rq && rq < photo_.rows && 0 <= cq && cq < photo_.cols) + { + // Get barycentric coordinates for the current point. + getBarycentricCoordiantes(v1, v2, v3, static_cast(cq)+cRoundOff, static_cast(rq)+rRoundOff, l1, l2, l3); + + // The z value for the point. + float z = v1z*l1+v2z*l2+v3z*l3; + + // Check depth + float depthValue = depth_.at(rq, cq); + if(z < depthValue) + { + // Current is behind last, don't draw. + continue; + } + + // The uv values of the point. + float u, v; + u = v1u*l1+v2u*l2+v3u*l3; + v = v1v*l1+v2v*l2+v3v*l3; + + // Nearest neighbour texture interpolation. + int uq = static_cast(u*fCols); + int vq = static_cast((1.0f-v)*fRows); + photo_.at(rq,cq) = texture.at(vq, uq); + + // Update depth buffer. + depth_.at(rq, cq) = z; + } + } + } + } +} + +void OdmOrthoPhoto::getBarycentricCoordiantes(pcl::PointXYZ v1, pcl::PointXYZ v2, pcl::PointXYZ v3, float x, float y, float &l1, float &l2, float &l3) const +{ + // Diff along y. + float y2y3 = v2.y-v3.y; + float y1y3 = v1.y-v3.y; + float y3y1 = v3.y-v1.y; + float yy3 = y -v3.y; + + // Diff along x. + float x3x2 = v3.x-v2.x; + float x1x3 = v1.x-v3.x; + float xx3 = x -v3.x; + + // Normalization factor. + float norm = (y2y3*x1x3 + x3x2*y1y3); + + l1 = (y2y3*(xx3) + x3x2*(yy3)) / norm; + l2 = (y3y1*(xx3) + x1x3*(yy3)) / norm; + l3 = 1 - l1 - l2; +} + +bool OdmOrthoPhoto::isSliverPolygon(pcl::PointXYZ v1, pcl::PointXYZ v2, pcl::PointXYZ v3) const +{ + // Calculations are made using doubles, to minize rounding errors. + Eigen::Vector3d a = Eigen::Vector3d(static_cast(v1.x), static_cast(v1.y), static_cast(v1.z)); + Eigen::Vector3d b = Eigen::Vector3d(static_cast(v2.x), static_cast(v2.y), static_cast(v2.z)); + Eigen::Vector3d c = Eigen::Vector3d(static_cast(v3.x), static_cast(v3.y), static_cast(v3.z)); + Eigen::Vector3d dummyVec = (a-b).cross(c-b); + + // Area smaller than, or equal to, floating-point epsilon. + return std::numeric_limits::epsilon() >= static_cast(std::sqrt(dummyVec.dot(dummyVec))/2.0); +} + +bool OdmOrthoPhoto::isModelOk(const pcl::TextureMesh &mesh) +{ + // The number of texture coordinates in the model. + size_t nTextureCoordinates = 0; + // The number of faces in the model. + size_t nFaces = 0; + + for(size_t t = 0; t < mesh.tex_coordinates.size(); ++t) + { + nTextureCoordinates += mesh.tex_coordinates[t].size(); + } + for(size_t t = 0; t < mesh.tex_polygons.size(); ++t) + { + nFaces += mesh.tex_polygons[t].size(); + } + + log_ << "Number of faces in the model " << nFaces << '\n'; + + return 3*nFaces == nTextureCoordinates; +} diff --git a/odm_orthophoto/src/OdmOrthoPhoto.hpp b/odm_orthophoto/src/OdmOrthoPhoto.hpp new file mode 100644 index 000000000..a4503fd9d --- /dev/null +++ b/odm_orthophoto/src/OdmOrthoPhoto.hpp @@ -0,0 +1,140 @@ +#pragma once + +// C++ +#include + +// PCL +#include +#include + +// OpenCV +#include +#include + +// PCL +#include +#include + +// OpenCV +#include + +// Logger +#include "Logger.hpp" + +/*! + * \brief The OdmOrthoPhoto class is used to create an orthograpic photo over a given area. + * The class reads an oriented textured mesh from an OBJ-file. + * The class uses file read from pcl. + * The class uses image read and write from opencv. + */ +class OdmOrthoPhoto +{ +public: + OdmOrthoPhoto(); + ~OdmOrthoPhoto(); + + /*! + * \brief run Runs the ortho photo functionality using the provided input arguments. + * For a list of accepted arguments, pleas see the main page documentation or + * call the program with parameter "-help". + * \param argc Application argument count. + * \param argv Argument values. + * \return 0 if successful. + */ + int run(int argc, char* argv[]); + +private: + + /*! + * \brief parseArguments Parses command line arguments. + * \param argc Application argument count. + * \param argv Argument values. + */ + void parseArguments(int argc, char* argv[]); + + /*! + * \brief printHelp Prints help, explaining usage. Can be shown by calling the program with argument: "-help". + */ + void printHelp(); + + /*! + * \brief Create the ortho photo using the current settings. + */ + void createOrthoPhoto(); + + /*! + * \brief Creates a transformation which aligns the area for the orthophoto. + */ + Eigen::Transform getROITransform(float xMin, float yMin) const; + + /*! + * \brief Renders a triangle into the ortho photo. + * \param texture The texture of the polygon. + * \param polygon The polygon as athree indices relative meshCloud. + * \param meshCloud Contains all vertices. + * \param uvs Contains the texture coordiantes for the active material. + * \param faceIndex The index of the face. + */ + void drawTexturedTriangle(const cv::Mat &texture, const pcl::Vertices &polygon, const pcl::PointCloud::Ptr &meshCloud, const std::vector &uvs, size_t faceIndex); + + /*! + * \brief Calcualtes the barycentric coordinates of a point in a triangle. + * \param v1 The first triangle vertex. + * \param v2 The second triangle vertex. + * \param v3 The third triangle vertex. + * \param x The x coordinate of the point. + * \param y The y coordinate of the point. + * \param l1 The first vertex weight. + * \param l2 The second vertex weight. + * \param l3 The third vertex weight. + */ + void getBarycentricCoordiantes(pcl::PointXYZ v1, pcl::PointXYZ v2, pcl::PointXYZ v3, float x, float y, float &l1, float &l2, float &l3) const; + + /*! + * \brief Check if a given polygon is a sliver polygon. + * \param v1 The first vertex of the polygon. + * \param v2 The second vertex of the polygon. + * \param v3 The third vertex of the polygon. + */ + bool isSliverPolygon(pcl::PointXYZ v1, pcl::PointXYZ v2, pcl::PointXYZ v3) const; + + /*! + * \brief Check if the model is suitable for ortho photo generation. + * \param mesh The + */ + bool isModelOk(const pcl::TextureMesh &mesh); + + Logger log_; /**< Logging object. */ + + std::string inputFile_; /**< Path to the textured mesh as an obj-file. */ + std::string outputFile_; /**< Path to the destination file. */ + std::string logFile_; /**< Path to the log file. */ + + float resolution_; /**< The number of pixels per meter in the ortho photo. */ + + Eigen::Vector2f boundryPoint1_; /**< The first boundry point for the ortho photo. */ + Eigen::Vector2f boundryPoint2_; /**< The second boundry point for the ortho photo. */ + Eigen::Vector2f boundryPoint3_; /**< The third boundry point for the ortho photo. */ + Eigen::Vector2f boundryPoint4_; /**< The fourth boundry point for the ortho photo. */ + + cv::Mat photo_; /**< The ortho photo as an OpenCV matrix, CV_8UC3. */ + cv::Mat depth_; /**< The depth of the ortho photo as an OpenCV matrix, CV_32F. */ + + bool multiMaterial_; /**< True if the mesh has multiple materials. **/ +}; + +/*! + * \brief The OdmOrthoPhoto class + */ +class OdmOrthoPhotoException : public std::exception +{ + +public: + OdmOrthoPhotoException() : message("Error in OdmOrthoPhoto") {} + OdmOrthoPhotoException(std::string msgInit) : message("Error in OdmOrthoPhoto:\n" + msgInit) {} + ~OdmOrthoPhotoException() throw() {} + virtual const char* what() const throw() {return message.c_str(); } + +private: + std::string message; /**< The error message **/ +}; diff --git a/odm_orthophoto/src/main.cpp b/odm_orthophoto/src/main.cpp new file mode 100644 index 000000000..1490915be --- /dev/null +++ b/odm_orthophoto/src/main.cpp @@ -0,0 +1,8 @@ +// Ortho photo generator. +#include "OdmOrthoPhoto.hpp" + +int main(int argc, char* argv[]) +{ + OdmOrthoPhoto orthoPhotoGenerator; + return orthoPhotoGenerator.run(argc, argv); +} diff --git a/odm_texturing/CMakeLists.txt b/odm_texturing/CMakeLists.txt new file mode 100644 index 000000000..bc66817ad --- /dev/null +++ b/odm_texturing/CMakeLists.txt @@ -0,0 +1,34 @@ +project(odm_texturing) +cmake_minimum_required(VERSION 2.8) + +# Set pcl dir to the input spedified with option -DPCL_DIR="path" +set(PCL_DIR "PCL_DIR-NOTFOUND" CACHE "PCL_DIR" "Path to the pcl installation directory") + +# Add compiler options. +add_definitions(-Wall -Wextra) + +# Find pcl at the location specified by PCL_DIR +find_package(PCL 1.7 HINTS "${PCL_DIR}/share/pcl-1.7" REQUIRED) + +# Find OpenCV at the default location +find_package(OpenCV REQUIRED) + +# Only link with required opencv modules. +set(OpenCV_LIBS opencv_core opencv_imgproc opencv_highgui) + +# Add the PCL, Eigen and OpenCV include dirs. +# Necessary since the PCL_INCLUDE_DIR variable set by find_package is broken.) +include_directories(${PCL_ROOT}/include/pcl-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}) +include_directories(${EIGEN_ROOT}) +include_directories(${OpenCV_INCLUDE_DIRS}) + +#library_directories(${OpenCV_LIBRARY_DIRS}) + +# Add source directory +aux_source_directory("./src" SRC_LIST) + +# Add exectuteable +add_executable(${PROJECT_NAME} ${SRC_LIST}) +target_link_libraries(odm_texturing ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_SURFACE_LIBRARIES} ${OpenCV_LIBS}) + + diff --git a/odm_texturing/src/Logger.cpp b/odm_texturing/src/Logger.cpp new file mode 100644 index 000000000..a6c81a8b5 --- /dev/null +++ b/odm_texturing/src/Logger.cpp @@ -0,0 +1,31 @@ +#include "Logger.hpp" + + +Logger::Logger(bool isPrintingInCout) : isPrintingInCout_(isPrintingInCout) +{ + +} + +Logger::~Logger() +{ + +} + +void Logger::printToFile(std::string filePath) +{ + std::ofstream file(filePath.c_str(), std::ios::binary); + file << logStream_.str(); + file.close(); +} + +bool Logger::isPrintingInCout() const +{ + return isPrintingInCout_; +} + +void Logger::setIsPrintingInCout(bool isPrintingInCout) +{ + isPrintingInCout_ = isPrintingInCout; +} + + diff --git a/odm_texturing/src/Logger.hpp b/odm_texturing/src/Logger.hpp new file mode 100644 index 000000000..31c5538cb --- /dev/null +++ b/odm_texturing/src/Logger.hpp @@ -0,0 +1,68 @@ +#pragma once + +// STL +#include +#include +#include +#include + +/*! + * \brief The Logger class is used to store program messages in a log file. + * \details By using the << operator while printInCout is set, the class writes both to + * cout and to file, if the flag is not set, output is written to file only. + */ +class Logger +{ +public: + /*! + * \brief Logger Contains functionality for printing and displaying log information. + * \param printInCout Flag toggling if operator << also writes to cout. + */ + Logger(bool isPrintingInCout = true); + + /*! + * \brief Destructor. + */ + ~Logger(); + + /*! + * \brief print Prints the contents of the log to file. + * \param filePath Path specifying where to write the log. + */ + void printToFile(std::string filePath); + + /*! + * \brief isPrintingInCout Check if console printing flag is set. + * \return Console printing flag. + */ + bool isPrintingInCout() const; + + /*! + * \brief setIsPrintingInCout Set console printing flag. + * \param isPrintingInCout Value, if true, messages added to the log are also printed in cout. + */ + void setIsPrintingInCout(bool isPrintingInCout); + + /*! + * Operator for printing messages to log and in the standard output stream if desired. + */ + template + friend Logger& operator<< (Logger &log, T t) + { + // If console printing is enabled. + if (log.isPrintingInCout_) + { + std::cout << t; + std::cout.flush(); + } + // Write to log. + log.logStream_ << t; + + return log; + } + +private: + bool isPrintingInCout_; /*!< If flag is set, log is printed in cout and written to the log. */ + + std::stringstream logStream_; /*!< Stream for storing the log. */ +}; diff --git a/odm_texturing/src/OdmTexturing.cpp b/odm_texturing/src/OdmTexturing.cpp new file mode 100644 index 000000000..bec1f9ccc --- /dev/null +++ b/odm_texturing/src/OdmTexturing.cpp @@ -0,0 +1,1116 @@ +#include "OdmTexturing.hpp" + +OdmTexturing::OdmTexturing() : log_(false) +{ + logFilePath_ = "odm_texturing_log.txt"; + + bundleResizedTo_ = 1200.0; + textureWithSize_ = 2000.0; + textureResolution_ = 4096.0; + nrTextures_ = 0; + padding_ = 15.0; + + mesh_ = pcl::TextureMeshPtr(new pcl::TextureMesh); + patches_ = std::vector(0); + tTIA_ = std::vector(0); + +} + +OdmTexturing::~OdmTexturing() +{ + +} + +int OdmTexturing::run(int argc, char **argv) +{ + if (argc <= 1) + { + printHelp(); + return EXIT_SUCCESS; + } + + try + { + parseArguments(argc, argv); + loadMesh(); + loadCameras(); + triangleToImageAssignment(); + calculatePatches(); + sortPatches(); + createTextures(); + writeObjFile(); + } + catch (const OdmTexturingException& e) + { + log_.setIsPrintingInCout(true); + log_ << "Error in OdmTexturing:\n"; + log_ << e.what() << "\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (const std::exception& e) + { + log_.setIsPrintingInCout(true); + log_ << "Error in OdmTexturing:\n"; + log_ << e.what() << "\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (...) + { + log_.setIsPrintingInCout(true); + log_ << "Unknown error in OdmTexturing:\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + + log_.printToFile(logFilePath_); + return EXIT_SUCCESS; +} + +void OdmTexturing::parseArguments(int argc, char** argv) +{ + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + // The argument to be parsed + std::string argument = std::string(argv[argIndex]); + if (argument == "-help") + { + printHelp(); + } + else if (argument == "-verbose") + { + log_.setIsPrintingInCout(true); + } + else if (argument == "-bundleFile") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + bundlePath_ = std::string(argv[argIndex]); + std::ifstream testFile(bundlePath_.c_str(), std::ios_base::binary); + if (!testFile.is_open()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value (file not accessible)."); + } + log_ << "Bundle path was set to: " << bundlePath_ << "\n"; + } + else if (argument == "-imagesPath") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> imagesPath_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "Images path was set to: " << imagesPath_ << "\n"; + } + else if (argument == "-imagesListPath") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + imagesListPath_ = std::string(argv[argIndex]); + std::ifstream testFile(imagesListPath_.c_str(), std::ios_base::binary); + if (!testFile.is_open()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value (file not accessible)."); + } + log_ << "Images list path was set to: " << imagesListPath_ << "\n"; + } + else if (argument == "-inputModelPath") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + inputModelPath_ = std::string(argv[argIndex]); + std::ifstream testFile(inputModelPath_.c_str(), std::ios_base::binary); + if (!testFile.is_open()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value (file not accessible)."); + + } + log_ << "Input model path was set to: " << inputModelPath_ << "\n"; + } + else if (argument == "-outputFolder") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> outputFolder_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "Output folder path was set to: " << outputFolder_ << "\n"; + } + else if (argument == "-logFile") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + logFilePath_ = std::string(argv[argIndex]); + std::ofstream testFile(logFilePath_.c_str()); + if (!testFile.is_open()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value."); + } + log_ << "Log file path was set to: " << logFilePath_ << "\n"; + } + else if (argument == "-textureResolution") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> textureResolution_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "The texture resolution was set to: " << textureResolution_ << "\n"; + } + else if (argument == "-textureWithSize") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> textureWithSize_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "The resolution to texture with was set to: " << textureWithSize_ << "\n"; + } + else if (argument == "-bundleResizedTo") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> bundleResizedTo_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "The resized resolution used in bundler was set to: " << bundleResizedTo_ << "\n"; + } + else + { + printHelp(); + throw OdmTexturingException("Unrecognized argument '" + argument + "'."); + } + } + + if (textureWithSize_ > textureResolution_) + { + textureWithSize_ = textureResolution_; + log_ << "textureWithSize parameter was set to a lower value since it can not be greater than the texture resolution.\n"; + } + +} + +void OdmTexturing::loadMesh() +{ + // Read model from ply-file + pcl::PolygonMeshPtr plyMeshPtr(new pcl::PolygonMesh); + if (pcl::io::loadPLYFile(inputModelPath_, *plyMeshPtr.get()) == -1) + { + throw OdmTexturingException("Error when reading model from:\n" + inputModelPath_ + "\n"); + } + else + { + log_ << "Successfully loaded " << plyMeshPtr->polygons.size() << " polygons from file.\n"; + } + + // Transfer data from ply file to TextureMesh + mesh_->cloud = plyMeshPtr->cloud; + std::vector polygons; + + // Push faces from ply-mesh into TextureMesh + polygons.resize(plyMeshPtr->polygons.size()); + for (size_t i = 0; i < plyMeshPtr->polygons.size(); ++i) + { + polygons[i] = plyMeshPtr->polygons[i]; + } + mesh_->tex_polygons.push_back(polygons); + +} + +void OdmTexturing::loadCameras() +{ + std::ifstream bundleFile, imageListFile; + bundleFile.open(bundlePath_.c_str(), std::ios_base::binary); + imageListFile.open(imagesListPath_.c_str(), std::ios_base::binary); + + // Check if file is open + if(!bundleFile.is_open()) + { + throw OdmTexturingException("Error when reading the bundle file."); + } + else + { + log_ << "Successfully read the bundle file.\n"; + } + + if (!imageListFile.is_open()) + { + throw OdmTexturingException("Error when reading the image list file."); + } + else + { + log_ << "Successfully read the image list file.\n"; + } + + // A temporary storage for a line from the file. + std::string dummyLine = ""; + + std::getline(bundleFile, dummyLine); + + int nrCameras= 0; + bundleFile >> nrCameras; + bundleFile >> dummyLine; + for (int i = 0; i < nrCameras;++i) + { + double val; + pcl::TextureMapping::Camera cam; + Eigen::Affine3f transform; + bundleFile >> val; //Read focal length from bundle file + cam.focal_length = val; + bundleFile >> val; //Read k1 from bundle file + bundleFile >> val; //Read k2 from bundle file + + bundleFile >> val; transform(0,0) = val; // Read rotation (0,0) from bundle file + bundleFile >> val; transform(0,1) = val; // Read rotation (0,1) from bundle file + bundleFile >> val; transform(0,2) = val; // Read rotation (0,2) from bundle file + + bundleFile >> val; transform(1,0) = val; // Read rotation (1,0) from bundle file + bundleFile >> val; transform(1,1) = val; // Read rotation (1,1) from bundle file + bundleFile >> val; transform(1,2) = val; // Read rotation (1,2) from bundle file + + bundleFile >> val; transform(2,0) = val; // Read rotation (2,0) from bundle file + bundleFile >> val; transform(2,1) = val; // Read rotation (2,1) from bundle file + bundleFile >> val; transform(2,2) = val; // Read rotation (2,2) from bundle file + + bundleFile >> val; transform(0,3) = val; // Read translation (0,3) from bundle file + bundleFile >> val; transform(1,3) = val; // Read translation (1,3) from bundle file + bundleFile >> val; transform(2,3) = val; // Read translation (2,3) from bundle file + + transform(3,0) = 0.0; + transform(3,1) = 0.0; + transform(3,2) = 0.0; + transform(3,3) = 1.0; + + transform = transform.inverse(); + + // Column negation + transform(0,2) = -1.0*transform(0,2); + transform(1,2) = -1.0*transform(1,2); + transform(2,2) = -1.0*transform(2,2); + + transform(0,1) = -1.0*transform(0,1); + transform(1,1) = -1.0*transform(1,1); + transform(2,1) = -1.0*transform(2,1); + + // Set values from bundle to current camera + cam.pose = transform; + + std::getline(imageListFile, dummyLine); + cam.texture_file = imagesPath_ + dummyLine.substr(2,dummyLine.length()); + + // Read image to get full resolution size + cv::Mat image = cv::imread(cam.texture_file); + + // Calculate scale factor to texture with textureWithSize + double factor = textureWithSize_/static_cast(image.cols); + if (factor > 1.0f) + { + factor = 1.0f; + } + + // Update camera size and focal length + cam.height = static_cast(std::floor(factor*(static_cast(image.rows)))); + cam.width = static_cast(std::floor(factor*(static_cast(image.cols)))); + cam.focal_length *= static_cast(cam.width)/bundleResizedTo_; + + // Add camera + cameras_.push_back(cam); + + } + +} + +void OdmTexturing::triangleToImageAssignment() +{ + // Resize the triangleToImageAssigmnent vector to match the number of faces in the mesh + tTIA_.resize(mesh_->tex_polygons[0].size()); + + // Set all values in the triangleToImageAssignment vector to a default value (-1) if there are no optimal camera + for (size_t i = 0; i < tTIA_.size(); ++i) + { + tTIA_[i] = -1; + } + + // Vector containing information if the face has been given an optimal camera or not + std::vector hasOptimalCamera = std::vector(mesh_->tex_polygons[0].size()); + + // Set default value that no face has an optimal camera + for (size_t faceIndex = 0; faceIndex < hasOptimalCamera.size(); ++faceIndex) + { + hasOptimalCamera[faceIndex] = false; + } + + // Convert vertices to pcl::PointXYZ cloud + pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (mesh_->cloud, *meshCloud); + + // Create dummy point and UV-index for vertices not visible in any cameras + pcl::PointXY nanPoint; + nanPoint.x = std::numeric_limits::quiet_NaN(); + nanPoint.y = std::numeric_limits::quiet_NaN(); + pcl::texture_mapping::UvIndex uvNull; + uvNull.idx_cloud = -1; + uvNull.idx_face = -1; + + for (size_t cameraIndex = 0; cameraIndex < cameras_.size(); ++cameraIndex) + { + // Move vertices in mesh into the camera coordinate system + pcl::PointCloud::Ptr cameraCloud (new pcl::PointCloud); + pcl::transformPointCloud (*meshCloud, *cameraCloud, cameras_[cameraIndex].pose.inverse()); + + // Cloud to contain points projected into current camera + pcl::PointCloud::Ptr projections (new pcl::PointCloud); + + // Vector containing information if the polygon is visible in current camera + std::vector visibility; + visibility.resize(mesh_->tex_polygons[0].size()); + + // Vector for remembering the correspondence between uv-coordinates and faces + std::vector indexUvToPoints; + + // Count the number of vertices inside the camera frustum + int countInsideFrustum = 0; + + // Frustum culling for all faces + for (size_t faceIndex = 0; faceIndex < mesh_->tex_polygons[0].size(); ++faceIndex) + { + // Variables for the face vertices as projections in the camera plane + pcl::PointXY pixelPos0; pcl::PointXY pixelPos1; pcl::PointXY pixelPos2; + + // If the face is inside the camera frustum + if (isFaceProjected(cameras_[cameraIndex], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[0]], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[1]], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[2]], + pixelPos0, pixelPos1, pixelPos2)) + { + // Add pixel positions in camera to projections + projections->points.push_back((pixelPos0)); + projections->points.push_back((pixelPos1)); + projections->points.push_back((pixelPos2)); + + // Remember corresponding face + pcl::texture_mapping::UvIndex u1, u2, u3; + u1.idx_cloud = mesh_->tex_polygons[0][faceIndex].vertices[0]; + u2.idx_cloud = mesh_->tex_polygons[0][faceIndex].vertices[1]; + u3.idx_cloud = mesh_->tex_polygons[0][faceIndex].vertices[2]; + u1.idx_face = faceIndex; u2.idx_face = faceIndex; u3.idx_face = faceIndex; + indexUvToPoints.push_back(u1); + indexUvToPoints.push_back(u2); + indexUvToPoints.push_back(u3); + + // Update visibility vector + visibility[faceIndex] = true; + + // Update count + ++countInsideFrustum; + } + else + { + // If not visible set nanPoint and uvNull + projections->points.push_back(nanPoint); + projections->points.push_back(nanPoint); + projections->points.push_back(nanPoint); + indexUvToPoints.push_back(uvNull); + indexUvToPoints.push_back(uvNull); + indexUvToPoints.push_back(uvNull); + + // Update visibility vector + visibility[faceIndex] = false; + } + + + } + + // If any faces are visible in the current camera perform occlusion culling + if (countInsideFrustum > 0) + { + // Set up acceleration structure + pcl::KdTreeFLANN kdTree; + kdTree.setInputCloud(projections); + + // Loop through all faces and perform occlusion culling for faces inside frustum + for (size_t faceIndex = 0; faceIndex < mesh_->tex_polygons[0].size(); ++faceIndex) + { + if (visibility[faceIndex]) + { + // Vectors to store output from radiusSearch in acceleration structure + std::vector neighbors; std::vector neighborsSquaredDistance; + + // Variables for the vertices in face as projections in the camera plane + pcl::PointXY pixelPos0; pcl::PointXY pixelPos1; pcl::PointXY pixelPos2; + + if (isFaceProjected(cameras_[cameraIndex], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[0]], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[1]], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[2]], + pixelPos0, pixelPos1, pixelPos2)) + { + // Variables for a radius circumscribing the polygon in the camera plane and the center of the polygon + double radius; pcl::PointXY center; + + // Get values for radius and center + getTriangleCircumscribedCircleCentroid(pixelPos0, pixelPos1, pixelPos2, center, radius); + + // Perform radius search in the acceleration structure + int radiusSearch = kdTree.radiusSearch(center, radius, neighbors, neighborsSquaredDistance); + + // If other projections are found inside the radius + if (radiusSearch > 0) + { + // Extract distances for all vertices for face to camera + double d0 = cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[0]].z; + double d1 = cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[1]].z; + double d2 = cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[2]].z; + + // Calculate largest distance and store in distance variable + double distance = std::max(d0, std::max(d1,d2)); + + // Compare distance to all neighbors inside radius + for (size_t i = 0; i < neighbors.size(); ++i) + { + // Distance variable from neighbor to camera + double neighborDistance = cameraCloud->points[indexUvToPoints[neighbors[i]].idx_cloud].z; + + // If the neighbor has a greater distance to the camera and is inside face polygon set it as not visible + if (distance < neighborDistance) + { + if (checkPointInsideTriangle(pixelPos0, pixelPos1, pixelPos2, projections->points[neighbors[i]])) + { + // Update visibility for neighbors + visibility[indexUvToPoints[neighbors[i]].idx_face] = false; + } + } + } + } + } + } + } + } + + // Number of polygons that add current camera as the optimal camera + int count = 0; + + // Update optimal cameras for faces visible in current camera + for (size_t faceIndex = 0; faceIndex < visibility.size();++faceIndex) + { + if (visibility[faceIndex]) + { + hasOptimalCamera[faceIndex] = true; + tTIA_[faceIndex] = cameraIndex; + ++count; + } + } + + } + +} + +void OdmTexturing::calculatePatches() +{ + // Convert vertices to pcl::PointXYZ cloud + pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (mesh_->cloud, *meshCloud); + + // Reserve size for patches_ + patches_.reserve(tTIA_.size()); + + // Vector containing vector with indicies to faces visible in corresponding camera index + std::vector > optFaceCameraVector = std::vector >(cameras_.size()); + + // Counter variables for visible and occluded faces + int countVis = 0; + int countOcc = 0; + + Patch nonVisibleFaces; + nonVisibleFaces.optimalCameraIndex_ = -1; + nonVisibleFaces.materialIndex_ = -1; + nonVisibleFaces.placed_ = true; + + // Setup vector containing vectors with all faces correspondning to camera according to triangleToImageAssignment vector + for (size_t i = 0; i < tTIA_.size(); ++i) + { + if (tTIA_[i] > -1) + { + // If face has an optimal camera add to optFaceCameraVector and update counter for visible faces + countVis++; + optFaceCameraVector[tTIA_[i]].push_back(i); + } + else + { + // Add non visible face to patch nonVisibleFaces + nonVisibleFaces.faces_.push_back(i); + + // Update counter for occluded faces + countOcc++; + } + } + + log_ << "Visible faces: "<::Ptr cameraCloud (new pcl::PointCloud); + pcl::transformPointCloud (*meshCloud, *cameraCloud, cameras_[cameraIndex].pose.inverse()); + + // While faces visible in camera remains to be assigned to a patch + while(0 < optFaceCameraVector[cameraIndex].size()) + { + // Create current patch + Patch patch; + + // Vector containing faces to check connectivity with current patch + std::vector addedFaces = std::vector(0); + + // Add last face in optFaceCameraVector to faces to check connectivity and add it to the current patch + addedFaces.push_back(optFaceCameraVector[cameraIndex].back()); + + // Add first face to patch + patch.faces_.push_back(optFaceCameraVector[cameraIndex].back()); + + // Remove face from optFaceCameraVector + optFaceCameraVector[cameraIndex].pop_back(); + + // Declare uv-coordinates for face + pcl::PointXY uvCoord1; pcl::PointXY uvCoord2; pcl::PointXY uvCoord3; + + // Calculate uv-coordinates for face in camera + if (isFaceProjected(cameras_[cameraIndex], + cameraCloud->points[mesh_->tex_polygons[0][addedFaces.back()].vertices[0]], + cameraCloud->points[mesh_->tex_polygons[0][addedFaces.back()].vertices[1]], + cameraCloud->points[mesh_->tex_polygons[0][addedFaces.back()].vertices[2]], + uvCoord1, uvCoord2, uvCoord3)) + { + // Set minimum and maximum uv-coordinate value for patch + patch.minu_ = std::min(uvCoord1.x, std::min(uvCoord2.x, uvCoord3.x)); + patch.minv_ = std::min(uvCoord1.y, std::min(uvCoord2.y, uvCoord3.y)); + patch.maxu_ = std::max(uvCoord1.x, std::max(uvCoord2.x, uvCoord3.x)); + patch.maxv_ = std::max(uvCoord1.y, std::max(uvCoord2.y, uvCoord3.y)); + + while(0 < addedFaces.size()) + { + // Set face to check neighbors + size_t patchFaceIndex = addedFaces.back(); + + // Remove patchFaceIndex from addedFaces + addedFaces.pop_back(); + + // Check against all remaining faces with the same optimal camera + for (size_t i = 0; i < optFaceCameraVector[cameraIndex].size(); ++i) + { + size_t modelFaceIndex = optFaceCameraVector[cameraIndex][i]; + + // Don't check against self + if (modelFaceIndex != patchFaceIndex) + { + // Store indices for vertices of both faces + size_t face0v0 = mesh_->tex_polygons[0][modelFaceIndex].vertices[0]; + size_t face0v1 = mesh_->tex_polygons[0][modelFaceIndex].vertices[1]; + size_t face0v2 = mesh_->tex_polygons[0][modelFaceIndex].vertices[2]; + size_t face1v0 = mesh_->tex_polygons[0][patchFaceIndex].vertices[0]; + size_t face1v1 = mesh_->tex_polygons[0][patchFaceIndex].vertices[1]; + size_t face1v2 = mesh_->tex_polygons[0][patchFaceIndex].vertices[2]; + + // Count the number of shared vertices + size_t nShared = 0; + nShared += (face0v0 == face1v0 ? 1 : 0) + (face0v0 == face1v1 ? 1 : 0) + (face0v0 == face1v2 ? 1 : 0); + nShared += (face0v1 == face1v0 ? 1 : 0) + (face0v1 == face1v1 ? 1 : 0) + (face0v1 == face1v2 ? 1 : 0); + nShared += (face0v2 == face1v0 ? 1 : 0) + (face0v2 == face1v1 ? 1 : 0) + (face0v2 == face1v2 ? 1 : 0); + + // If sharing a vertex + if (nShared > 0) + { + // Declare uv-coordinates for face + pcl::PointXY uv1; pcl::PointXY uv2; pcl::PointXY uv3; + + // Calculate uv-coordinates for face in camera + isFaceProjected(cameras_[cameraIndex], + cameraCloud->points[mesh_->tex_polygons[0][modelFaceIndex].vertices[0]], + cameraCloud->points[mesh_->tex_polygons[0][modelFaceIndex].vertices[1]], + cameraCloud->points[mesh_->tex_polygons[0][modelFaceIndex].vertices[2]], + uv1, uv2, uv3); + + // Update minimum and maximum uv-coordinate value for patch + patch.minu_ = std::min(patch.minu_, std::min(uv1.x, std::min(uv2.x, uv3.x))); + patch.minv_ = std::min(patch.minv_, std::min(uv1.y, std::min(uv2.y, uv3.y))); + patch.maxu_ = std::max(patch.maxu_, std::max(uv1.x, std::max(uv2.x, uv3.x))); + patch.maxv_ = std::max(patch.maxv_, std::max(uv1.y, std::max(uv2.y, uv3.y))); + + // Add modelFaceIndex to patch + patch.faces_.push_back(modelFaceIndex); + + // Add modelFaceIndex from faces to check for neighbors with same optimal camera + addedFaces.push_back(modelFaceIndex); + + // Remove modelFaceIndex from optFaceCameraVector to exclude it from comming iterations + optFaceCameraVector[cameraIndex].erase(optFaceCameraVector[cameraIndex].begin() + i); + } + } + } + } + } + + // Set optimal camera for patch + patch.optimalCameraIndex_ = static_cast(cameraIndex); + + // Add patch to patches_ vector + patches_.push_back(patch); + } + } + patches_.push_back(nonVisibleFaces); +} + +Coords OdmTexturing::recursiveFindCoords(Node &n, float w, float h) +{ + // Coordinates to return and place patch + Coords c; + + if (NULL != n.lft_) + { + c = recursiveFindCoords(*(n.lft_), w, h); + if (c.success_) + { + return c; + } + else + { + return recursiveFindCoords(*(n.rgt_), w, h); + } + } + else + { + // If the patch is to large or occupied return success false for coord + if (n.used_ || w > n.width_ || h > n.height_) + { + c.success_ = false; + return c; + } + + // If the patch matches perfectly, store it + if (w == n.width_ && h == n.height_) + { + n.used_ = true; + c.r_ = n.r_; + c.c_ = n.c_; + c.success_ = true; + + return c; + } + + // Initialize children for node + n.lft_ = new Node(n); + n.rgt_ = new Node(n); + + n.rgt_->used_ = false; + n.lft_->used_ = false; + n.rgt_->rgt_ = NULL; + n.rgt_->lft_ = NULL; + n.lft_->rgt_ = NULL; + n.lft_->lft_ = NULL; + + // Check how to adjust free space + if (n.width_ - w > n.height_ - h) + { + n.lft_->width_ = w; + n.rgt_->c_ = n.c_ + w; + n.rgt_->width_ = n.width_ - w; + } + else + { + n.lft_->height_ = h; + n.rgt_->r_ = n.r_ + h; + n.rgt_->height_ = n.height_ - h; + } + + return recursiveFindCoords(*(n.lft_), w, h); + } +} + +void OdmTexturing::sortPatches() +{ + // Bool to set true when done + bool done = false; + + // Material index + int materialIndex = 0; + + // Number of patches left from last loop + size_t countLeftLastIteration = 0; + + while(!done) + { + // Create container for current material + Node root; + root.width_ = textureResolution_; + root.height_ = textureResolution_; + + // Set done to true + done = true; + + // Number of patches that did not fit in current material + size_t countNotPlacedPatches = 0; + + // Number of patches placed in current material + size_t placed = 0; + + for (size_t patchIndex = 0; patchIndex < patches_.size(); ++patchIndex) + { + if(!patches_[patchIndex].placed_) + { + // Calculate dimensions of the patch + float w = patches_[patchIndex].maxu_ - patches_[patchIndex].minu_ + 2*padding_; + float h = patches_[patchIndex].maxv_ - patches_[patchIndex].minv_ + 2*padding_; + + // Try to place patch in root container for this material + if ( w > 0.0 && h > 0.0) + { + patches_[patchIndex].c_ = recursiveFindCoords(root, w, h); + } + + if (!patches_[patchIndex].c_.success_) + { + ++countNotPlacedPatches; + done = false; + } + else + { + // Set patch material as current material + patches_[patchIndex].materialIndex_ = materialIndex; + + // Set patch as placed + patches_[patchIndex].placed_ = true; + + // Update number of patches placed in current material + placed++; + + // Update patch with padding_ + //patches_[patchIndex].c_.c_ += padding_; + //patches_[patchIndex].c_.r_ += padding_; + patches_[patchIndex].minu_ -= padding_; + patches_[patchIndex].minv_ -= padding_; + patches_[patchIndex].maxu_ = std::min((patches_[patchIndex].maxu_ + padding_), textureResolution_); + patches_[patchIndex].maxv_ = std::min((patches_[patchIndex].maxv_ + padding_), textureResolution_); + } + } + } + + // Update material index + ++materialIndex; + + if (countLeftLastIteration == countNotPlacedPatches && countNotPlacedPatches != 0) + { + done = true; + } + countLeftLastIteration = countNotPlacedPatches; + } + + // Set number of textures + nrTextures_ = materialIndex; + + log_ << "Faces sorted into " << nrTextures_ << " textures.\n"; +} + +void OdmTexturing::createTextures() +{ + // Convert vertices to pcl::PointXYZ cloud + pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (mesh_->cloud, *meshCloud); + + // Container for faces according to submesh. Used to replace faces in mesh_. + std::vector > faceVector = std::vector >(nrTextures_ + 1); + + // Container for texture coordinates according to submesh. Used to replace texture coordinates in mesh_. + std::vector > textureCoordinatesVector = std::vector >(nrTextures_ + 1); + + // Container for materials according to submesh. Used to replace materials in mesh_. + std::vector materialVector = std::vector(nrTextures_ + 1); + + // Setup model according to patches placement + for (int textureIndex = 0; textureIndex < nrTextures_; ++textureIndex) + { + for (size_t patchIndex = 0; patchIndex < patches_.size(); ++patchIndex) + { + // If patch is placed in current mesh add all containing faces to that submesh + if (patches_[patchIndex].materialIndex_ == textureIndex) + { + // Transform mesh into camera + pcl::PointCloud::Ptr cameraCloud (new pcl::PointCloud); + pcl::transformPointCloud (*meshCloud, *cameraCloud, cameras_[patches_[patchIndex].optimalCameraIndex_].pose.inverse()); + + // Loop through all faces in patch + for (size_t faceIndex = 0; faceIndex < patches_[patchIndex].faces_.size(); ++faceIndex) + { + // Setup global face index in mesh_ + size_t globalFaceIndex = patches_[patchIndex].faces_[faceIndex]; + + // Add current face to current submesh + faceVector[textureIndex].push_back(mesh_->tex_polygons[0][globalFaceIndex]); + + // Pixel positions + pcl::PointXY pixelPos0; pcl::PointXY pixelPos1; pcl::PointXY pixelPos2; + + // Get pixel positions in corresponding camera for the vertices of the face + getPixelCoordinates(cameraCloud->points[mesh_->tex_polygons[0][globalFaceIndex].vertices[0]], cameras_[patches_[patchIndex].optimalCameraIndex_], pixelPos0); + getPixelCoordinates(cameraCloud->points[mesh_->tex_polygons[0][globalFaceIndex].vertices[1]], cameras_[patches_[patchIndex].optimalCameraIndex_], pixelPos1); + getPixelCoordinates(cameraCloud->points[mesh_->tex_polygons[0][globalFaceIndex].vertices[2]], cameras_[patches_[patchIndex].optimalCameraIndex_], pixelPos2); + + // Shorthands for patch variables + float c = patches_[patchIndex].c_.c_ + padding_; + float r = patches_[patchIndex].c_.r_ + padding_; + float minu = patches_[patchIndex].minu_ + padding_; + float minv = patches_[patchIndex].minv_ + padding_; + + // Declare uv coordinates + Eigen::Vector2f uv1, uv2, uv3; + + // Set uv coordinates according to patch + uv1(0) = (pixelPos0.x - minu + c)/textureResolution_; + uv1(1) = 1.0f - (pixelPos0.y - minv + r)/textureResolution_; + + uv2(0) = (pixelPos1.x - minu + c)/textureResolution_; + uv2(1) = 1.0f - (pixelPos1.y - minv + r)/textureResolution_; + + uv3(0) = (pixelPos2.x - minu + c)/textureResolution_; + uv3(1) = 1.0f - (pixelPos2.y - minv + r)/textureResolution_; + + // Add uv coordinates to submesh + textureCoordinatesVector[textureIndex].push_back(uv1); + textureCoordinatesVector[textureIndex].push_back(uv2); + textureCoordinatesVector[textureIndex].push_back(uv3); + } + } + } + + // Declare material and setup default values + pcl::TexMaterial meshMaterial; + meshMaterial.tex_Ka.r = 0.0f; meshMaterial.tex_Ka.g = 0.0f; meshMaterial.tex_Ka.b = 0.0f; + meshMaterial.tex_Kd.r = 0.0f; meshMaterial.tex_Kd.g = 0.0f; meshMaterial.tex_Kd.b = 0.0f; + meshMaterial.tex_Ks.r = 0.0f; meshMaterial.tex_Ks.g = 0.0f; meshMaterial.tex_Ks.b = 0.0f; + meshMaterial.tex_d = 1.0f; meshMaterial.tex_Ns = 200.0f; meshMaterial.tex_illum = 2; + std::stringstream tex_name; + tex_name << "texture_" << textureIndex; + tex_name >> meshMaterial.tex_name; + meshMaterial.tex_file = meshMaterial.tex_name + ".jpg"; + materialVector[textureIndex] = meshMaterial; + } + + // Add non visible patches to submesh + for (size_t patchIndex = 0; patchIndex < patches_.size(); ++patchIndex) + { + // If the patch does not have an optimal camera + if (patches_[patchIndex].optimalCameraIndex_ == -1) + { + // Add all faces and set uv coordinates + for (size_t faceIndex = 0; faceIndex < patches_[patchIndex].faces_.size(); ++faceIndex) + { + // Setup global face index in mesh_ + size_t globalFaceIndex = patches_[patchIndex].faces_[faceIndex]; + + // Add current face to current submesh + faceVector[nrTextures_].push_back(mesh_->tex_polygons[0][globalFaceIndex]); + + // Declare uv coordinates + Eigen::Vector2f uv1, uv2, uv3; + + // Set uv coordinates according to patch + uv1(0) = 0.25f;//(pixelPos0.x - minu + c)/textureResolution_; + uv1(1) = 0.25f;//1.0f - (pixelPos0.y - minv + r)/textureResolution_; + + uv2(0) = 0.25f;//(pixelPos1.x - minu + c)/textureResolution_; + uv2(1) = 0.75f;//1.0f - (pixelPos1.y - minv + r)/textureResolution_; + + uv3(0) = 0.75f;//(pixelPos2.x - minu + c)/textureResolution_; + uv3(1) = 0.75f;//1.0f - (pixelPos2.y - minv + r)/textureResolution_; + + // Add uv coordinates to submesh + textureCoordinatesVector[nrTextures_].push_back(uv1); + textureCoordinatesVector[nrTextures_].push_back(uv2); + textureCoordinatesVector[nrTextures_].push_back(uv3); + } + } + } + + // Declare material and setup default values for nonVisibileFaces submesh + pcl::TexMaterial meshMaterial; + meshMaterial.tex_Ka.r = 0.0f; meshMaterial.tex_Ka.g = 0.0f; meshMaterial.tex_Ka.b = 0.0f; + meshMaterial.tex_Kd.r = 0.0f; meshMaterial.tex_Kd.g = 0.0f; meshMaterial.tex_Kd.b = 0.0f; + meshMaterial.tex_Ks.r = 0.0f; meshMaterial.tex_Ks.g = 0.0f; meshMaterial.tex_Ks.b = 0.0f; + meshMaterial.tex_d = 1.0f; meshMaterial.tex_Ns = 200.0f; meshMaterial.tex_illum = 2; + std::stringstream tex_name; + tex_name << "non_visible_faces_texture"; + tex_name >> meshMaterial.tex_name; + meshMaterial.tex_file = meshMaterial.tex_name + ".jpg"; + materialVector[nrTextures_] = meshMaterial; + + // Replace polygons, texture coordinates and materials in mesh_ + mesh_->tex_polygons = faceVector; + mesh_->tex_coordinates = textureCoordinatesVector; + mesh_->tex_materials = materialVector; + + // Containers for image and the resized image used for texturing + cv::Mat image; + cv::Mat resizedImage; + + for (int textureIndex = 0; textureIndex < nrTextures_; ++textureIndex) + { + // Current texture for corresponding material + cv::Mat texture = cv::Mat::zeros(textureResolution_, textureResolution_, CV_8UC3); + + for (int cameraIndex = 0; cameraIndex < static_cast(cameras_.size()); ++cameraIndex) + { + // Load image for current camera + image = cv::imread(cameras_[cameraIndex].texture_file,1); + + // Calculate the resize factor to texturize with textureWithSize_ + double resizeFactor = textureWithSize_ / static_cast(image.cols); + + if (resizeFactor > 1.0f) + { + resizeFactor = 1.0f; + } + + // Resize image to the resolution used to texture with + cv::resize(image, resizedImage, cv::Size(), resizeFactor, resizeFactor, CV_INTER_AREA); + + // Loop through all patches + for (size_t patchIndex = 0; patchIndex < patches_.size(); ++patchIndex) + { + // If the patch has the current camera as optimal camera + if (patches_[patchIndex].materialIndex_ == textureIndex && patches_[patchIndex].optimalCameraIndex_ == cameraIndex) + { + // Pixel coordinates to extract image information from + int extractX = static_cast(floor(patches_[patchIndex].minu_));// + padding_); + int extractY = static_cast(floor(patches_[patchIndex].minv_));// + padding_); + + // Pixel coordinates to insert the image information to + int insertX = static_cast(floor(patches_[patchIndex].c_.c_)); + int insertY = static_cast(floor(patches_[patchIndex].c_.r_)); + + // The size of the image information to use + int width = static_cast(floor(patches_[patchIndex].maxu_)) - extractX - 1; + int height = static_cast(floor(patches_[patchIndex].maxv_)) - extractY - 1; + + // Get image information and add to texture + cv::Mat src = resizedImage(cv::Rect(extractX, extractY, width, height)); + cv::Mat dst = texture(cv::Rect(insertX, insertY, width, height)); + src.copyTo(dst); + } + } + } + log_ <<"Saved " << mesh_->tex_materials[textureIndex].tex_file <<" to file.\n"; + cv::imwrite(outputFolder_ + mesh_->tex_materials[textureIndex].tex_file, texture); + } + + // Create nonVisibleFaces texture and save to file + cv::Mat nonVisibleFacesTexture = cv::Mat::zeros(50, 50, CV_8UC3) + cv::Scalar(255,255,255); + log_ <<"Saved " << mesh_->tex_materials[nrTextures_].tex_file <<" to file.\n"; + cv::imwrite(outputFolder_ + mesh_->tex_materials[nrTextures_].tex_file, nonVisibleFacesTexture); +} + +void OdmTexturing::writeObjFile() +{ + if (saveOBJFile(outputFolder_ + "odm_textured_model.obj", *mesh_.get(), 7) == 0) + { + log_ <<"odm_textured_model.obj successfully saved.\n"; + } + else + { + log_ << "Failed to save model.\n"; + } +} + +void OdmTexturing::printHelp() +{ + log_.setIsPrintingInCout(true); + + log_ << "Purpose:\n"; + log_ << "Create a textured mesh from pmvs output and a corresponding ply-mesh from OpenDroneMap-meshing.\n"; + + log_ << "Usage:\n"; + log_ << "The program requires paths to a bundler.out file, a ply model file, an image list file, an image folder path, an output path and the resized resolution used in the bundler step.\n"; + log_ << "All other input parameters are optional.\n\n"; + + log_ << "The following flags are available:\n"; + log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n"; + log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output.\n\n"; + + log_ << "Parameters are specified as: \"- \", (without <>), and the following parameters are configurable:\n"; + log_ << "\"-bundleFile \" (mandatory)\n"; + log_ << "Path to the bundle.out file from pmvs.\n"; + + log_ << "\"-imagesListPath \" (mandatory)\n"; + log_ << "Path to the list containing the image names used in the bundle.out file.\n"; + + log_ << "\"-imagesPath \" (mandatory)\n"; + log_ << "Path to the folder containing full resolution images.\n"; + + log_ << "\"-inputModelPath \" (mandatory)\n"; + log_ << "Path to the ply model to texture.\n"; + + log_ << "\"-outputFolder \" (mandatory)\n"; + log_ << "Path to store the textured model. The folder must exist.\n"; + + log_ << "\"-logFile \" (optional)\n"; + log_ << "Path to save the log file.\n"; + + log_ << "\"-bundleResizedTo \" (mandatory)\n"; + log_ << "The resized resolution used in bundler.\n"; + + log_ << "\"-textureResolution_ \" (optional, default: 4096)\n"; + log_ << "The resolution of the output textures. Must be greater than textureWithSize.\n"; + + log_ << "\"-textureWithSize \" (optional, default: 1200)\n"; + log_ << "The resolution to rescale the images performing the texturing.\n"; + + log_.setIsPrintingInCout(false); + +} diff --git a/odm_texturing/src/OdmTexturing.cpp~ b/odm_texturing/src/OdmTexturing.cpp~ new file mode 100644 index 000000000..98aa9a017 --- /dev/null +++ b/odm_texturing/src/OdmTexturing.cpp~ @@ -0,0 +1,1116 @@ +#include "OdmTexturing.hpp" + +OdmTexturing::OdmTexturing() : log_(false) +{ + logFilePath_ = "odm_texturing_log.txt"; + + bundleResizedTo_ = 1200.0; + textureWithSize_ = 2000.0; + textureResolution_ = 4096.0; + nrTextures_ = 0; + padding_ = 15.0; + + mesh_ = pcl::TextureMeshPtr(new pcl::TextureMesh); + patches_ = std::vector(0); + tTIA_ = std::vector(0); + +} + +OdmTexturing::~OdmTexturing() +{ + +} + +int OdmTexturing::run(int argc, char **argv) +{ + if (argc <= 1) + { + printHelp(); + return EXIT_SUCCESS; + } + + try + { + parseArguments(argc, argv); + loadMesh(); + loadCameras(); + triangleToImageAssignment(); + calculatePatches(); + sortPatches(); + createTextures(); + writeObjFile(); + } + catch (const OdmTexturingException& e) + { + log_.setIsPrintingInCout(true); + log_ << "Error in OdmTexturing:\n"; + log_ << e.what() << "\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (const std::exception& e) + { + log_.setIsPrintingInCout(true); + log_ << "Error in OdmTexturing:\n"; + log_ << e.what() << "\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + catch (...) + { + log_.setIsPrintingInCout(true); + log_ << "Unknown error in OdmTexturing:\n"; + log_.printToFile(logFilePath_); + log_ << "For more detailed information, see log file." << "\n"; + return EXIT_FAILURE; + } + + log_.printToFile(logFilePath_); + return EXIT_SUCCESS; +} + +void OdmTexturing::parseArguments(int argc, char** argv) +{ + for(int argIndex = 1; argIndex < argc; ++argIndex) + { + // The argument to be parsed + std::string argument = std::string(argv[argIndex]); + if (argument == "-help") + { + printHelp(); + } + else if (argument == "-verbose") + { + log_.setIsPrintingInCout(true); + } + else if (argument == "-bundleFile") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + bundlePath_ = std::string(argv[argIndex]); + std::ifstream testFile(bundlePath_.c_str(), std::ios_base::binary); + if (!testFile.is_open()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value (file not accessible)."); + } + log_ << "Bundle path was set to: " << bundlePath_ << "\n"; + } + else if (argument == "-imagesPath") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> imagesPath_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "Images path was set to: " << imagesPath_ << "\n"; + } + else if (argument == "-imagesListPath") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + imagesListPath_ = std::string(argv[argIndex]); + std::ifstream testFile(imagesListPath_.c_str(), std::ios_base::binary); + if (!testFile.is_open()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value (file not accessible)."); + } + log_ << "Images list path was set to: " << imagesListPath_ << "\n"; + } + else if (argument == "-inputModelPath") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + inputModelPath_ = std::string(argv[argIndex]); + std::ifstream testFile(inputModelPath_.c_str(), std::ios_base::binary); + if (!testFile.is_open()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value (file not accessible)."); + + } + log_ << "Input model path was set to: " << inputModelPath_ << "\n"; + } + else if (argument == "-outputFolder") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> outputFolder_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "Output folder path was set to: " << outputFolder_ << "\n"; + } + else if (argument == "-logFile") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + logFilePath_ = std::string(argv[argIndex]); + std::ofstream testFile(logFilePath_.c_str()); + if (!testFile.is_open()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value."); + } + log_ << "Log file path was set to: " << logFilePath_ << "\n"; + } + else if (argument == "-textureResolution") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> textureResolution_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "The texture resolution was set to: " << textureResolution_ << "\n"; + } + else if (argument == "-textureWithSize") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> textureWithSize_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "The resolution to texture with was set to: " << textureWithSize_ << "\n"; + } + else if (argument == "-bundleResizedTo") + { + ++argIndex; + if (argIndex >= argc) + { + throw OdmTexturingException("Missing argument for '" + argument + "'."); + } + std::stringstream ss(argv[argIndex]); + ss >> bundleResizedTo_; + if (ss.bad()) + { + throw OdmTexturingException("Argument '" + argument + "' has a bad value. (wrong type)"); + } + log_ << "The resized resolution used in bundler was set to: " << bundleResizedTo_ << "\n"; + } + else + { + printHelp(); + throw OdmTexturingException("Unrecognized argument '" + argument + "'."); + } + } + + if (textureWithSize_ > textureResolution_) + { + textureWithSize_ = textureResolution_; + log_ << "textureWithSize parameter was set to a lower value since it can not be greater than the texture resolution.\n"; + } + +} + +void OdmTexturing::loadMesh() +{ + // Read model from ply-file + pcl::PolygonMeshPtr plyMeshPtr(new pcl::PolygonMesh); + if (pcl::io::loadPLYFile(inputModelPath_, *plyMeshPtr.get()) == -1) + { + throw OdmTexturingException("Error when reading model from:\n" + inputModelPath_ + "\n"); + } + else + { + log_ << "Successfully loaded " << plyMeshPtr->polygons.size() << " polygons from file.\n"; + } + + // Transfer data from ply file to TextureMesh + mesh_->cloud = plyMeshPtr->cloud; + std::vector polygons; + + // Push faces from ply-mesh into TextureMesh + polygons.resize(plyMeshPtr->polygons.size()); + for (size_t i = 0; i < plyMeshPtr->polygons.size(); ++i) + { + polygons[i] = plyMeshPtr->polygons[i]; + } + mesh_->tex_polygons.push_back(polygons); + +} + +void OdmTexturing::loadCameras() +{ + std::ifstream bundleFile, imageListFile; + bundleFile.open(bundlePath_.c_str(), std::ios_base::binary); + imageListFile.open(imagesListPath_.c_str(), std::ios_base::binary); + + // Check if file is open + if(!bundleFile.is_open()) + { + throw OdmTexturingException("Error when reading the bundle file."); + } + else + { + log_ << "Successfully read the bundle file.\n"; + } + + if (!imageListFile.is_open()) + { + throw OdmTexturingException("Error when reading the image list file."); + } + else + { + log_ << "Successfully read the image list file.\n"; + } + + // A temporary storage for a line from the file. + std::string dummyLine = ""; + + std::getline(bundleFile, dummyLine); + + int nrCameras= 0; + bundleFile >> nrCameras; + bundleFile >> dummyLine; + for (int i = 0; i < nrCameras;++i) + { + double val; + pcl::TextureMapping::Camera cam; + Eigen::Affine3f transform; + bundleFile >> val; //Read focal length from bundle file + cam.focal_length = val; + bundleFile >> val; //Read k1 from bundle file + bundleFile >> val; //Read k2 from bundle file + + bundleFile >> val; transform(0,0) = val; // Read rotation (0,0) from bundle file + bundleFile >> val; transform(0,1) = val; // Read rotation (0,1) from bundle file + bundleFile >> val; transform(0,2) = val; // Read rotation (0,2) from bundle file + + bundleFile >> val; transform(1,0) = val; // Read rotation (1,0) from bundle file + bundleFile >> val; transform(1,1) = val; // Read rotation (1,1) from bundle file + bundleFile >> val; transform(1,2) = val; // Read rotation (1,2) from bundle file + + bundleFile >> val; transform(2,0) = val; // Read rotation (2,0) from bundle file + bundleFile >> val; transform(2,1) = val; // Read rotation (2,1) from bundle file + bundleFile >> val; transform(2,2) = val; // Read rotation (2,2) from bundle file + + bundleFile >> val; transform(0,3) = val; // Read translation (0,3) from bundle file + bundleFile >> val; transform(1,3) = val; // Read translation (1,3) from bundle file + bundleFile >> val; transform(2,3) = val; // Read translation (2,3) from bundle file + + transform(3,0) = 0.0; + transform(3,1) = 0.0; + transform(3,2) = 0.0; + transform(3,3) = 1.0; + + transform = transform.inverse(); + + // Column negation + transform(0,2) = -1.0*transform(0,2); + transform(1,2) = -1.0*transform(1,2); + transform(2,2) = -1.0*transform(2,2); + + transform(0,1) = -1.0*transform(0,1); + transform(1,1) = -1.0*transform(1,1); + transform(2,1) = -1.0*transform(2,1); + + // Set values from bundle to current camera + cam.pose = transform; + + std::getline(imageListFile, dummyLine); + cam.texture_file = imagesPath_ + dummyLine.substr(2,dummyLine.length()); + + // Read image to get full resolution size + cv::Mat image = cv::imread(cam.texture_file); + + // Calculate scale factor to texture with textureWithSize + double factor = textureWithSize_/static_cast(image.cols); + if (factor > 1.0f) + { + factor = 1.0f; + } + + // Update camera size and focal length + cam.height = static_cast(std::floor(factor*(static_cast(image.rows)))); + cam.width = static_cast(std::floor(factor*(static_cast(image.cols)))); + cam.focal_length *= static_cast(cam.width)/bundleResizedTo_; + + // Add camera + cameras_.push_back(cam); + + } + +} + +void OdmTexturing::triangleToImageAssignment() +{ + // Resize the triangleToImageAssigmnent vector to match the number of faces in the mesh + tTIA_.resize(mesh_->tex_polygons[0].size()); + + // Set all values in the triangleToImageAssignment vector to a default value (-1) if there are no optimal camera + for (size_t i = 0; i < tTIA_.size(); ++i) + { + tTIA_[i] = -1; + } + + // Vector containing information if the face has been given an optimal camera or not + std::vector hasOptimalCamera = std::vector(mesh_->tex_polygons[0].size()); + + // Set default value that no face has an optimal camera + for (size_t faceIndex = 0; faceIndex < hasOptimalCamera.size(); ++faceIndex) + { + hasOptimalCamera[faceIndex] = false; + } + + // Convert vertices to pcl::PointXYZ cloud + pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (mesh_->cloud, *meshCloud); + + // Create dummy point and UV-index for vertices not visible in any cameras + pcl::PointXY nanPoint; + nanPoint.x = std::numeric_limits::quiet_NaN(); + nanPoint.y = std::numeric_limits::quiet_NaN(); + pcl::texture_mapping::UvIndex uvNull; + uvNull.idx_cloud = -1; + uvNull.idx_face = -1; + + for (size_t cameraIndex = 0; cameraIndex < cameras_.size(); ++cameraIndex) + { + // Move vertices in mesh into the camera coordinate system + pcl::PointCloud::Ptr cameraCloud (new pcl::PointCloud); + pcl::transformPointCloud (*meshCloud, *cameraCloud, cameras_[cameraIndex].pose.inverse()); + + // Cloud to contain points projected into current camera + pcl::PointCloud::Ptr projections (new pcl::PointCloud); + + // Vector containing information if the polygon is visible in current camera + std::vector visibility; + visibility.resize(mesh_->tex_polygons[0].size()); + + // Vector for remembering the correspondence between uv-coordinates and faces + std::vector indexUvToPoints; + + // Count the number of vertices inside the camera frustum + int countInsideFrustum = 0; + + // Frustum culling for all faces + for (size_t faceIndex = 0; faceIndex < mesh_->tex_polygons[0].size(); ++faceIndex) + { + // Variables for the face vertices as projections in the camera plane + pcl::PointXY pixelPos0; pcl::PointXY pixelPos1; pcl::PointXY pixelPos2; + + // If the face is inside the camera frustum + if (isFaceProjected(cameras_[cameraIndex], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[0]], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[1]], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[2]], + pixelPos0, pixelPos1, pixelPos2)) + { + // Add pixel positions in camera to projections + projections->points.push_back((pixelPos0)); + projections->points.push_back((pixelPos1)); + projections->points.push_back((pixelPos2)); + + // Remember corresponding face + pcl::texture_mapping::UvIndex u1, u2, u3; + u1.idx_cloud = mesh_->tex_polygons[0][faceIndex].vertices[0]; + u2.idx_cloud = mesh_->tex_polygons[0][faceIndex].vertices[1]; + u3.idx_cloud = mesh_->tex_polygons[0][faceIndex].vertices[2]; + u1.idx_face = faceIndex; u2.idx_face = faceIndex; u3.idx_face = faceIndex; + indexUvToPoints.push_back(u1); + indexUvToPoints.push_back(u2); + indexUvToPoints.push_back(u3); + + // Update visibility vector + visibility[faceIndex] = true; + + // Update count + ++countInsideFrustum; + } + else + { + // If not visible set nanPoint and uvNull + projections->points.push_back(nanPoint); + projections->points.push_back(nanPoint); + projections->points.push_back(nanPoint); + indexUvToPoints.push_back(uvNull); + indexUvToPoints.push_back(uvNull); + indexUvToPoints.push_back(uvNull); + + // Update visibility vector + visibility[faceIndex] = false; + } + + + } + + // If any faces are visible in the current camera perform occlusion culling + if (countInsideFrustum > 0) + { + // Set up acceleration structure + pcl::KdTreeFLANN kdTree; + kdTree.setInputCloud(projections); + + // Loop through all faces and perform occlusion culling for faces inside frustum + for (size_t faceIndex = 0; faceIndex < mesh_->tex_polygons[0].size(); ++faceIndex) + { + if (visibility[faceIndex]) + { + // Vectors to store output from radiusSearch in acceleration structure + std::vector neighbors; std::vector neighborsSquaredDistance; + + // Variables for the vertices in face as projections in the camera plane + pcl::PointXY pixelPos0; pcl::PointXY pixelPos1; pcl::PointXY pixelPos2; + + if (isFaceProjected(cameras_[cameraIndex], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[0]], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[1]], + cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[2]], + pixelPos0, pixelPos1, pixelPos2)) + { + // Variables for a radius circumscribing the polygon in the camera plane and the center of the polygon + double radius; pcl::PointXY center; + + // Get values for radius and center + getTriangleCircumscribedCircleCentroid(pixelPos0, pixelPos1, pixelPos2, center, radius); + + // Perform radius search in the acceleration structure + int radiusSearch = kdTree.radiusSearch(center, radius, neighbors, neighborsSquaredDistance); + + // If other projections are found inside the radius + if (radiusSearch > 0) + { + // Extract distances for all vertices for face to camera + double d0 = cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[0]].z; + double d1 = cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[1]].z; + double d2 = cameraCloud->points[mesh_->tex_polygons[0][faceIndex].vertices[2]].z; + + // Calculate largest distance and store in distance variable + double distance = std::max(d0, std::max(d1,d2)); + + // Compare distance to all neighbors inside radius + for (size_t i = 0; i < neighbors.size(); ++i) + { + // Distance variable from neighbor to camera + double neighborDistance = cameraCloud->points[indexUvToPoints[neighbors[i]].idx_cloud].z; + + // If the neighbor has a greater distance to the camera and is inside face polygon set it as not visible + if (distance < neighborDistance) + { + if (checkPointInsideTriangle(pixelPos0, pixelPos1, pixelPos2, projections->points[neighbors[i]])) + { + // Update visibility for neighbors + visibility[indexUvToPoints[neighbors[i]].idx_face] = false; + } + } + } + } + } + } + } + } + + // Number of polygons that add current camera as the optimal camera + int count = 0; + + // Update optimal cameras for faces visible in current camera + for (size_t faceIndex = 0; faceIndex < visibility.size();++faceIndex) + { + if (visibility[faceIndex]) + { + hasOptimalCamera[faceIndex] = true; + tTIA_[faceIndex] = cameraIndex; + ++count; + } + } + + } + +} + +void OdmTexturing::calculatePatches() +{ + // Convert vertices to pcl::PointXYZ cloud + pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (mesh_->cloud, *meshCloud); + + // Reserve size for patches_ + patches_.reserve(tTIA_.size()); + + // Vector containing vector with indicies to faces visible in corresponding camera index + std::vector > optFaceCameraVector = std::vector >(cameras_.size()); + + // Counter variables for visible and occluded faces + int countVis = 0; + int countOcc = 0; + + Patch nonVisibleFaces; + nonVisibleFaces.optimalCameraIndex_ = -1; + nonVisibleFaces.materialIndex_ = -1; + nonVisibleFaces.placed_ = true; + + // Setup vector containing vectors with all faces correspondning to camera according to triangleToImageAssignment vector + for (size_t i = 0; i < tTIA_.size(); ++i) + { + if (tTIA_[i] > -1) + { + // If face has an optimal camera add to optFaceCameraVector and update counter for visible faces + countVis++; + optFaceCameraVector[tTIA_[i]].push_back(i); + } + else + { + // Add non visible face to patch nonVisibleFaces + nonVisibleFaces.faces_.push_back(i); + + // Update counter for occluded faces + countOcc++; + } + } + + log_ << "Visible faces: "<::Ptr cameraCloud (new pcl::PointCloud); + pcl::transformPointCloud (*meshCloud, *cameraCloud, cameras_[cameraIndex].pose.inverse()); + + // While faces visible in camera remains to be assigned to a patch + while(0 < optFaceCameraVector[cameraIndex].size()) + { + // Create current patch + Patch patch; + + // Vector containing faces to check connectivity with current patch + std::vector addedFaces = std::vector(0); + + // Add last face in optFaceCameraVector to faces to check connectivity and add it to the current patch + addedFaces.push_back(optFaceCameraVector[cameraIndex].back()); + + // Add first face to patch + patch.faces_.push_back(optFaceCameraVector[cameraIndex].back()); + + // Remove face from optFaceCameraVector + optFaceCameraVector[cameraIndex].pop_back(); + + // Declare uv-coordinates for face + pcl::PointXY uvCoord1; pcl::PointXY uvCoord2; pcl::PointXY uvCoord3; + + // Calculate uv-coordinates for face in camera + if (isFaceProjected(cameras_[cameraIndex], + cameraCloud->points[mesh_->tex_polygons[0][addedFaces.back()].vertices[0]], + cameraCloud->points[mesh_->tex_polygons[0][addedFaces.back()].vertices[1]], + cameraCloud->points[mesh_->tex_polygons[0][addedFaces.back()].vertices[2]], + uvCoord1, uvCoord2, uvCoord3)) + { + // Set minimum and maximum uv-coordinate value for patch + patch.minu_ = std::min(uvCoord1.x, std::min(uvCoord2.x, uvCoord3.x)); + patch.minv_ = std::min(uvCoord1.y, std::min(uvCoord2.y, uvCoord3.y)); + patch.maxu_ = std::max(uvCoord1.x, std::max(uvCoord2.x, uvCoord3.x)); + patch.maxv_ = std::max(uvCoord1.y, std::max(uvCoord2.y, uvCoord3.y)); + + while(0 < addedFaces.size()) + { + // Set face to check neighbors + size_t patchFaceIndex = addedFaces.back(); + + // Remove patchFaceIndex from addedFaces + addedFaces.pop_back(); + + // Check against all remaining faces with the same optimal camera + for (size_t i = 0; i < optFaceCameraVector[cameraIndex].size(); ++i) + { + size_t modelFaceIndex = optFaceCameraVector[cameraIndex][i]; + + // Don't check against self + if (modelFaceIndex != patchFaceIndex) + { + // Store indices for vertices of both faces + size_t face0v0 = mesh_->tex_polygons[0][modelFaceIndex].vertices[0]; + size_t face0v1 = mesh_->tex_polygons[0][modelFaceIndex].vertices[1]; + size_t face0v2 = mesh_->tex_polygons[0][modelFaceIndex].vertices[2]; + size_t face1v0 = mesh_->tex_polygons[0][patchFaceIndex].vertices[0]; + size_t face1v1 = mesh_->tex_polygons[0][patchFaceIndex].vertices[1]; + size_t face1v2 = mesh_->tex_polygons[0][patchFaceIndex].vertices[2]; + + // Count the number of shared vertices + size_t nShared = 0; + nShared += (face0v0 == face1v0 ? 1 : 0) + (face0v0 == face1v1 ? 1 : 0) + (face0v0 == face1v2 ? 1 : 0); + nShared += (face0v1 == face1v0 ? 1 : 0) + (face0v1 == face1v1 ? 1 : 0) + (face0v1 == face1v2 ? 1 : 0); + nShared += (face0v2 == face1v0 ? 1 : 0) + (face0v2 == face1v1 ? 1 : 0) + (face0v2 == face1v2 ? 1 : 0); + + // If sharing a vertex + if (nShared > 0) + { + // Declare uv-coordinates for face + pcl::PointXY uv1; pcl::PointXY uv2; pcl::PointXY uv3; + + // Calculate uv-coordinates for face in camera + isFaceProjected(cameras_[cameraIndex], + cameraCloud->points[mesh_->tex_polygons[0][modelFaceIndex].vertices[0]], + cameraCloud->points[mesh_->tex_polygons[0][modelFaceIndex].vertices[1]], + cameraCloud->points[mesh_->tex_polygons[0][modelFaceIndex].vertices[2]], + uv1, uv2, uv3); + + // Update minimum and maximum uv-coordinate value for patch + patch.minu_ = std::min(patch.minu_, std::min(uv1.x, std::min(uv2.x, uv3.x))); + patch.minv_ = std::min(patch.minv_, std::min(uv1.y, std::min(uv2.y, uv3.y))); + patch.maxu_ = std::max(patch.maxu_, std::max(uv1.x, std::max(uv2.x, uv3.x))); + patch.maxv_ = std::max(patch.maxv_, std::max(uv1.y, std::max(uv2.y, uv3.y))); + + // Add modelFaceIndex to patch + patch.faces_.push_back(modelFaceIndex); + + // Add modelFaceIndex from faces to check for neighbors with same optimal camera + addedFaces.push_back(modelFaceIndex); + + // Remove modelFaceIndex from optFaceCameraVector to exclude it from comming iterations + optFaceCameraVector[cameraIndex].erase(optFaceCameraVector[cameraIndex].begin() + i); + } + } + } + } + } + + // Set optimal camera for patch + patch.optimalCameraIndex_ = static_cast(cameraIndex); + + // Add patch to patches_ vector + patches_.push_back(patch); + } + } + patches_.push_back(nonVisibleFaces); +} + +Coords OdmTexturing::recursiveFindCoords(Node &n, float w, float h) +{ + // Coordinates to return and place patch + Coords c; + + if (NULL != n.lft_) + { + c = recursiveFindCoords(*(n.lft_), w, h); + if (c.success_) + { + return c; + } + else + { + return recursiveFindCoords(*(n.rgt_), w, h); + } + } + else + { + // If the patch is to large or occupied return success false for coord + if (n.used_ || w > n.width_ || h > n.height_) + { + c.success_ = false; + return c; + } + + // If the patch matches perfectly, store it + if (w == n.width_ && h == n.height_) + { + n.used_ = true; + c.r_ = n.r_; + c.c_ = n.c_; + c.success_ = true; + + return c; + } + + // Initialize children for node + n.lft_ = new Node(n); + n.rgt_ = new Node(n); + + n.rgt_->used_ = false; + n.lft_->used_ = false; + n.rgt_->rgt_ = NULL; + n.rgt_->lft_ = NULL; + n.lft_->rgt_ = NULL; + n.lft_->lft_ = NULL; + + // Check how to adjust free space + if (n.width_ - w > n.height_ - h) + { + n.lft_->width_ = w; + n.rgt_->c_ = n.c_ + w; + n.rgt_->width_ = n.width_ - w; + } + else + { + n.lft_->height_ = h; + n.rgt_->r_ = n.r_ + h; + n.rgt_->height_ = n.height_ - h; + } + + return recursiveFindCoords(*(n.lft_), w, h); + } +} + +void OdmTexturing::sortPatches() +{ + // Bool to set true when done + bool done = false; + + // Material index + int materialIndex = 0; + + // Number of patches left from last loop + size_t countLeftLastIteration = 0; + + while(!done) + { + // Create container for current material + Node root; + root.width_ = textureResolution_; + root.height_ = textureResolution_; + + // Set done to true + done = true; + + // Number of patches that did not fit in current material + size_t countNotPlacedPatches = 0; + + // Number of patches placed in current material + size_t placed = 0; + + for (size_t patchIndex = 0; patchIndex < patches_.size(); ++patchIndex) + { + if(!patches_[patchIndex].placed_) + { + // Calculate dimensions of the patch + float w = patches_[patchIndex].maxu_ - patches_[patchIndex].minu_ + 2*padding_; + float h = patches_[patchIndex].maxv_ - patches_[patchIndex].minv_ + 2*padding_; + + // Try to place patch in root container for this material + if ( w > 0.0 && h > 0.0) + { + patches_[patchIndex].c_ = recursiveFindCoords(root, w, h); + } + + if (!patches_[patchIndex].c_.success_) + { + ++countNotPlacedPatches; + done = false; + } + else + { + // Set patch material as current material + patches_[patchIndex].materialIndex_ = materialIndex; + + // Set patch as placed + patches_[patchIndex].placed_ = true; + + // Update number of patches placed in current material + placed++; + + // Update patch with padding_ + //patches_[patchIndex].c_.c_ += padding_; + //patches_[patchIndex].c_.r_ += padding_; + patches_[patchIndex].minu_ -= padding_; + patches_[patchIndex].minv_ -= padding_; + patches_[patchIndex].maxu_ = std::min((patches_[patchIndex].maxu_ + padding_), textureResolution_); + patches_[patchIndex].maxv_ = std::min((patches_[patchIndex].maxv_ + padding_), textureResolution_); + } + } + } + + // Update material index + ++materialIndex; + + if (countLeftLastIteration == countNotPlacedPatches && countNotPlacedPatches != 0) + { + done = true; + } + countLeftLastIteration = countNotPlacedPatches; + } + + // Set number of textures + nrTextures_ = materialIndex; + + log_ << "Faces sorted into " << nrTextures_ << " textures.\n"; +} + +void OdmTexturing::createTextures() +{ + // Convert vertices to pcl::PointXYZ cloud + pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (mesh_->cloud, *meshCloud); + + // Container for faces according to submesh. Used to replace faces in mesh_. + std::vector > faceVector = std::vector >(nrTextures_ + 1); + + // Container for texture coordinates according to submesh. Used to replace texture coordinates in mesh_. + std::vector > textureCoordinatesVector = std::vector >(nrTextures_ + 1); + + // Container for materials according to submesh. Used to replace materials in mesh_. + std::vector materialVector = std::vector(nrTextures_ + 1); + + // Setup model according to patches placement + for (int textureIndex = 0; textureIndex < nrTextures_; ++textureIndex) + { + for (size_t patchIndex = 0; patchIndex < patches_.size(); ++patchIndex) + { + // If patch is placed in current mesh add all containing faces to that submesh + if (patches_[patchIndex].materialIndex_ == textureIndex) + { + // Transform mesh into camera + pcl::PointCloud::Ptr cameraCloud (new pcl::PointCloud); + pcl::transformPointCloud (*meshCloud, *cameraCloud, cameras_[patches_[patchIndex].optimalCameraIndex_].pose.inverse()); + + // Loop through all faces in patch + for (size_t faceIndex = 0; faceIndex < patches_[patchIndex].faces_.size(); ++faceIndex) + { + // Setup global face index in mesh_ + size_t globalFaceIndex = patches_[patchIndex].faces_[faceIndex]; + + // Add current face to current submesh + faceVector[textureIndex].push_back(mesh_->tex_polygons[0][globalFaceIndex]); + + // Pixel positions + pcl::PointXY pixelPos0; pcl::PointXY pixelPos1; pcl::PointXY pixelPos2; + + // Get pixel positions in corresponding camera for the vertices of the face + getPixelCoordinates(cameraCloud->points[mesh_->tex_polygons[0][globalFaceIndex].vertices[0]], cameras_[patches_[patchIndex].optimalCameraIndex_], pixelPos0); + getPixelCoordinates(cameraCloud->points[mesh_->tex_polygons[0][globalFaceIndex].vertices[1]], cameras_[patches_[patchIndex].optimalCameraIndex_], pixelPos1); + getPixelCoordinates(cameraCloud->points[mesh_->tex_polygons[0][globalFaceIndex].vertices[2]], cameras_[patches_[patchIndex].optimalCameraIndex_], pixelPos2); + + // Shorthands for patch variables + float c = patches_[patchIndex].c_.c_ + padding_; + float r = patches_[patchIndex].c_.r_ + padding_; + float minu = patches_[patchIndex].minu_ + padding_; + float minv = patches_[patchIndex].minv_ + padding_; + + // Declare uv coordinates + Eigen::Vector2f uv1, uv2, uv3; + + // Set uv coordinates according to patch + uv1(0) = (pixelPos0.x - minu + c)/textureResolution_; + uv1(1) = 1.0f - (pixelPos0.y - minv + r)/textureResolution_; + + uv2(0) = (pixelPos1.x - minu + c)/textureResolution_; + uv2(1) = 1.0f - (pixelPos1.y - minv + r)/textureResolution_; + + uv3(0) = (pixelPos2.x - minu + c)/textureResolution_; + uv3(1) = 1.0f - (pixelPos2.y - minv + r)/textureResolution_; + + // Add uv coordinates to submesh + textureCoordinatesVector[textureIndex].push_back(uv1); + textureCoordinatesVector[textureIndex].push_back(uv2); + textureCoordinatesVector[textureIndex].push_back(uv3); + } + } + } + + // Declare material and setup default values + pcl::TexMaterial meshMaterial; + meshMaterial.tex_Ka.r = 0.0f; meshMaterial.tex_Ka.g = 0.0f; meshMaterial.tex_Ka.b = 0.0f; + meshMaterial.tex_Kd.r = 0.0f; meshMaterial.tex_Kd.g = 0.0f; meshMaterial.tex_Kd.b = 0.0f; + meshMaterial.tex_Ks.r = 0.0f; meshMaterial.tex_Ks.g = 0.0f; meshMaterial.tex_Ks.b = 0.0f; + meshMaterial.tex_d = 1.0f; meshMaterial.tex_Ns = 200.0f; meshMaterial.tex_illum = 2; + std::stringstream tex_name; + tex_name << "texture_" << textureIndex; + tex_name >> meshMaterial.tex_name; + meshMaterial.tex_file = meshMaterial.tex_name + ".jpg"; + materialVector[textureIndex] = meshMaterial; + } + + // Add non visible patches to submesh + for (size_t patchIndex = 0; patchIndex < patches_.size(); ++patchIndex) + { + // If the patch does not have an optimal camera + if (patches_[patchIndex].optimalCameraIndex_ == -1) + { + // Add all faces and set uv coordinates + for (size_t faceIndex = 0; faceIndex < patches_[patchIndex].faces_.size(); ++faceIndex) + { + // Setup global face index in mesh_ + size_t globalFaceIndex = patches_[patchIndex].faces_[faceIndex]; + + // Add current face to current submesh + faceVector[nrTextures_].push_back(mesh_->tex_polygons[0][globalFaceIndex]); + + // Declare uv coordinates + Eigen::Vector2f uv1, uv2, uv3; + + // Set uv coordinates according to patch + uv1(0) = 0.25f;//(pixelPos0.x - minu + c)/textureResolution_; + uv1(1) = 0.25f;//1.0f - (pixelPos0.y - minv + r)/textureResolution_; + + uv2(0) = 0.25f;//(pixelPos1.x - minu + c)/textureResolution_; + uv2(1) = 0.75f;//1.0f - (pixelPos1.y - minv + r)/textureResolution_; + + uv3(0) = 0.75f;//(pixelPos2.x - minu + c)/textureResolution_; + uv3(1) = 0.75f;//1.0f - (pixelPos2.y - minv + r)/textureResolution_; + + // Add uv coordinates to submesh + textureCoordinatesVector[nrTextures_].push_back(uv1); + textureCoordinatesVector[nrTextures_].push_back(uv2); + textureCoordinatesVector[nrTextures_].push_back(uv3); + } + } + } + + // Declare material and setup default values for nonVisibileFaces submesh + pcl::TexMaterial meshMaterial; + meshMaterial.tex_Ka.r = 0.0f; meshMaterial.tex_Ka.g = 0.0f; meshMaterial.tex_Ka.b = 0.0f; + meshMaterial.tex_Kd.r = 0.0f; meshMaterial.tex_Kd.g = 0.0f; meshMaterial.tex_Kd.b = 0.0f; + meshMaterial.tex_Ks.r = 0.0f; meshMaterial.tex_Ks.g = 0.0f; meshMaterial.tex_Ks.b = 0.0f; + meshMaterial.tex_d = 1.0f; meshMaterial.tex_Ns = 200.0f; meshMaterial.tex_illum = 2; + std::stringstream tex_name; + tex_name << "non_visible_faces_texture"; + tex_name >> meshMaterial.tex_name; + meshMaterial.tex_file = meshMaterial.tex_name + ".jpg"; + materialVector[nrTextures_] = meshMaterial; + + // Replace polygons, texture coordinates and materials in mesh_ + mesh_->tex_polygons = faceVector; + mesh_->tex_coordinates = textureCoordinatesVector; + mesh_->tex_materials = materialVector; + + // Containers for image and the resized image used for texturing + cv::Mat image; + cv::Mat resizedImage; + + for (int textureIndex = 0; textureIndex < nrTextures_; ++textureIndex) + { + // Current texture for corresponding material + cv::Mat texture = cv::Mat::zeros(textureResolution_, textureResolution_, CV_8UC3); + + for (int cameraIndex = 0; cameraIndex < static_cast(cameras_.size()); ++cameraIndex) + { + // Load image for current camera + image = cv::imread(cameras_[cameraIndex].texture_file,1); + + // Calculate the resize factor to texturize with textureWithSize_ + double resizeFactor = textureWithSize_ / static_cast(image.cols); + + if (resizeFactor > 1.0f) + { + resizeFactor = 1.0f; + } + + // Resize image to the resolution used to texture with + cv::resize(image, resizedImage, cv::Size(), resizeFactor, resizeFactor, CV_INTER_AREA); + + // Loop through all patches + for (size_t patchIndex = 0; patchIndex < patches_.size(); ++patchIndex) + { + // If the patch has the current camera as optimal camera + if (patches_[patchIndex].materialIndex_ == textureIndex && patches_[patchIndex].optimalCameraIndex_ == cameraIndex) + { + // Pixel coordinates to extract image information from + int extractX = static_cast(floor(patches_[patchIndex].minu_));// + padding_); + int extractY = static_cast(floor(patches_[patchIndex].minv_));// + padding_); + + // Pixel coordinates to insert the image information to + int insertX = static_cast(floor(patches_[patchIndex].c_.c_)); + int insertY = static_cast(floor(patches_[patchIndex].c_.r_)); + + // The size of the image information to use + int width = static_cast(floor(patches_[patchIndex].maxu_)) - extractX - 1; + int height = static_cast(floor(patches_[patchIndex].maxv_)) - extractY - 1; + + // Get image information and add to texture + cv::Mat src = resizedImage(cv::Rect(extractX, extractY, width, height)); + cv::Mat dst = texture(cv::Rect(insertX, insertY, width, height)); + src.copyTo(dst); + } + } + } + log_ <<"Saved " << mesh_->tex_materials[textureIndex].tex_file <<" to file.\n"; + cv::imwrite(outputFolder_ + mesh_->tex_materials[textureIndex].tex_file, texture); + } + + // Create nonVisibleFaces texture and save to file + cv::Mat nonVisibleFacesTexture = cv::Mat::zeros(50, 50, CV_8UC3); + log_ <<"Saved " << mesh_->tex_materials[nrTextures_].tex_file <<" to file.\n"; + cv::imwrite(outputFolder_ + mesh_->tex_materials[nrTextures_].tex_file, nonVisibleFacesTexture); +} + +void OdmTexturing::writeObjFile() +{ + if (saveOBJFile(outputFolder_ + "odm_textured_model.obj", *mesh_.get(), 7) == 0) + { + log_ <<"odm_textured_model.obj successfully saved.\n"; + } + else + { + log_ << "Failed to save model.\n"; + } +} + +void OdmTexturing::printHelp() +{ + log_.setIsPrintingInCout(true); + + log_ << "Purpose:\n"; + log_ << "Create a textured mesh from pmvs output and a corresponding ply-mesh from OpenDroneMap-meshing.\n"; + + log_ << "Usage:\n"; + log_ << "The program requires paths to a bundler.out file, a ply model file, an image list file, an image folder path, an output path and the resized resolution used in the bundler step.\n"; + log_ << "All other input parameters are optional.\n\n"; + + log_ << "The following flags are available:\n"; + log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n"; + log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output.\n\n"; + + log_ << "Parameters are specified as: \"- \", (without <>), and the following parameters are configurable:\n"; + log_ << "\"-bundleFile \" (mandatory)\n"; + log_ << "Path to the bundle.out file from pmvs.\n"; + + log_ << "\"-imagesListPath \" (mandatory)\n"; + log_ << "Path to the list containing the image names used in the bundle.out file.\n"; + + log_ << "\"-imagesPath \" (mandatory)\n"; + log_ << "Path to the folder containing full resolution images.\n"; + + log_ << "\"-inputModelPath \" (mandatory)\n"; + log_ << "Path to the ply model to texture.\n"; + + log_ << "\"-outputFolder \" (mandatory)\n"; + log_ << "Path to store the textured model. The folder must exist.\n"; + + log_ << "\"-logFile \" (optional)\n"; + log_ << "Path to save the log file.\n"; + + log_ << "\"-bundleResizedTo \" (mandatory)\n"; + log_ << "The resized resolution used in bundler.\n"; + + log_ << "\"-textureResolution_ \" (optional, default: 4096)\n"; + log_ << "The resolution of the output textures. Must be greater than textureWithSize.\n"; + + log_ << "\"-textureWithSize \" (optional, default: 1200)\n"; + log_ << "The resolution to rescale the images performing the texturing.\n"; + + log_.setIsPrintingInCout(false); + +} diff --git a/odm_texturing/src/OdmTexturing.hpp b/odm_texturing/src/OdmTexturing.hpp new file mode 100644 index 000000000..1e884d222 --- /dev/null +++ b/odm_texturing/src/OdmTexturing.hpp @@ -0,0 +1,211 @@ +#pragma once + +// STL +#include +#include + +// PCL +#include +#include +#include + +// OpenCV +#include +#include + +// Modified PCL functions +#include "modifiedPclFunctions.hpp" + +// Logging +#include "Logger.hpp" + +/*! + * \brief The Coords struct Coordinate class used in recursiveFindCoordinates for OdmTexturing::sortPatches(). + */ +struct Coords +{ + // Coordinates for row and column + float r_, c_; + + // If coordinates have been placed + bool success_; + + Coords() + { + r_ = 0.0; + c_ = 0.0; + success_ = false; + } +}; + +/*! + * \brief The Patch struct Struct to hold all faces connected and with the same optimal camera. + */ +struct Patch +{ + std::vector faces_; + float minu_, minv_, maxu_, maxv_; + Coords c_; + bool placed_; + int materialIndex_; + int optimalCameraIndex_; + + Patch() + { + placed_ = false; + faces_ = std::vector(0); + minu_ = std::numeric_limits::infinity(); + minv_ = std::numeric_limits::infinity(); + maxu_ = 0.0; + maxv_ = 0.0; + optimalCameraIndex_ = -1; + materialIndex_ = 0; + } +}; + +/*! + * \brief The Node struct Node class for acceleration structure in OdmTexturing::sortPatches(). + */ +struct Node +{ + float r_, c_, width_, height_; + bool used_; + Node* rgt_; + Node* lft_; + + Node() + { + r_ = 0.0; + c_ = 0.0; + width_ = 1.0; + height_ = 1.0; + used_ = false; + rgt_ = NULL; + lft_ = NULL; + } + + Node(const Node &n) + { + r_ = n.r_; + c_ = n.c_; + used_ = n.used_; + width_ = n.width_; + height_ = n.height_; + rgt_ = n.rgt_; + lft_ = n.lft_; + } +}; + +/*! + * \brief The OdmTexturing class is used to create textures to a welded ply-mesh using the camera + * positions from pmvs as input. The result is stored in an obj-file with corresponding + * mtl-file and the textures saved as jpg. + */ +class OdmTexturing +{ +public: + OdmTexturing(); + ~OdmTexturing(); + + + /*! + * \brief run Runs the texturing functionality using the provided input arguments. + * For a list of the accepted arguments, please see the main page documentation or + * call the program with parameter "-help". + * \param argc Application argument count. + * \param argv Argument values. + * \return 0 if successful. + */ + int run(int argc, char **argv); + +private: + + /*! + * \brief parseArguments Parses command line arguments. + * \param argc Application argument count. + * \param argv Argument values. + */ + void parseArguments(int argc, char** argv); + + /*! + * \brief loadMesh Loads a PLY-file containing vertices and faces. + */ + void loadMesh(); + + /*! + * \brief loadCameras Loads cameras from a bundle.out file with corresponding image list file. + */ + void loadCameras(); + + /*! + * \brief triangleToImageAssignment Assigns optimal camera to faces for the faces that are visible. + */ + void triangleToImageAssignment(); + + /*! + * \brief calculatePatches Arrange faces into patches as a prestep to arranging UV-mapping. + */ + void calculatePatches(); + + /*! + * \brief recursiveFindCoords Recursive function used in sortPatches() to find free area to place patch. + * \param n The container in which to check for free space in. + * \param w The width of the box to place. + * \param h The height of the box to place. + * \return The coordinates where the patch has been placed. + */ + Coords recursiveFindCoords(Node &n, float w, float h); + + /*! + * \brief sortPatches Sorts patches into UV-containers to be used in createTextures() using a rectangle packer approach. + */ + void sortPatches(); + + /*! + * \brief createTextures Creates textures to the mesh. + */ + void createTextures(); + + /*! + * \brief writeObjFile Writes the textured mesh to file on the OBJ format. + */ + void writeObjFile(); + + /*! + * \brief printHelp Prints help, explaining usage. Can be shown by calling the program with arguments: "-help". + */ + void printHelp(); + + Logger log_; /**< Logging object. */ + std::string logFilePath_; /**< Path to store the log file. */ + + std::string bundlePath_; /**< Path to the bundle.out file. */ + std::string imagesPath_; /**< Path to the folder with all images in the image list. */ + std::string imagesListPath_; /**< Path to the image list. */ + std::string inputModelPath_; /**< Path to the ply-file containing the mesh to be textured. */ + std::string outputFolder_; /**< Path to the folder to store the output mesh and textures. */ + + double bundleResizedTo_; /**< The size used in the previous steps to calculate the camera focal_length. */ + double textureWithSize_; /**< The desired size of the images to texture with. */ + double textureResolution_; /**< The resolution of each texture. */ + double padding_; /**< A padding used to handle edge cases. */ + int nrTextures_; /**< The number of textures created. */ + + pcl::TextureMesh::Ptr mesh_; /**< PCL Texture Mesh */ + std::vector patches_; /**< The vector containing all patches */ + pcl::texture_mapping::CameraVector cameras_; /**< The vector containing all cameras. */ + std::vector tTIA_; /**< The vector containing the optimal cameras for all faces. */ +}; + +class OdmTexturingException : public std::exception +{ + +public: + OdmTexturingException() : message("Error in OdmTexturing") {} + OdmTexturingException(std::string msgInit) : message("Error in OdmTexturing:\n" + msgInit) {} + ~OdmTexturingException() throw() {} + virtual const char* what() const throw() {return message.c_str(); } + +private: + std::string message; /**< The error message. */ +}; diff --git a/odm_texturing/src/main.cpp b/odm_texturing/src/main.cpp new file mode 100644 index 000000000..36f9758e1 --- /dev/null +++ b/odm_texturing/src/main.cpp @@ -0,0 +1,15 @@ +// Include texturing source code +#include "OdmTexturing.hpp" + +/*! + * \mainpage main OpenDroneMap Texturing Module + * + * + * + */ + +int main (int argc, char** argv) +{ + OdmTexturing textureCreator; + return textureCreator.run(argc, argv); +} diff --git a/odm_texturing/src/modifiedPclFunctions.cpp b/odm_texturing/src/modifiedPclFunctions.cpp new file mode 100644 index 000000000..eafa94068 --- /dev/null +++ b/odm_texturing/src/modifiedPclFunctions.cpp @@ -0,0 +1,336 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2012-, Open Perception, Inc. +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder(s) nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +*/ + +#include "modifiedPclFunctions.hpp" + +int saveOBJFile(const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision) +{ + if (tex_mesh.cloud.data.empty ()) + { + PCL_ERROR ("[pcl::io::saveOBJFile] Input point cloud has no data!\n"); + return (-1); + } + + // Open file + std::ofstream fs; + fs.precision (precision); + fs.open (file_name.c_str ()); + + // Define material file + std::string mtl_file_name = file_name.substr (0, file_name.find_last_of (".")) + ".mtl"; + // Strip path for "mtllib" command + std::string mtl_file_name_nopath = mtl_file_name; + //std::cout << mtl_file_name_nopath << std::endl; + mtl_file_name_nopath.erase (0, mtl_file_name.find_last_of ('/') + 1); + + /* Write 3D information */ + // number of points + int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height; + int point_size = tex_mesh.cloud.data.size () / nr_points; + + // mesh size + int nr_meshes = tex_mesh.tex_polygons.size (); + // number of faces for header + int nr_faces = 0; + for (int m = 0; m < nr_meshes; ++m) + nr_faces += tex_mesh.tex_polygons[m].size (); + + // Write the header information + fs << "####" << std::endl; + fs << "# OBJ dataFile simple version. File name: " << file_name << std::endl; + fs << "# Vertices: " << nr_points << std::endl; + fs << "# Faces: " < 0) + f_idx += tex_mesh.tex_polygons[m-1].size (); + + if(tex_mesh.tex_materials.size() !=0) + { + fs << "# The material will be used for mesh " << m << std::endl; + //TODO pbl here with multi texture and unseen faces + fs << "usemtl " << tex_mesh.tex_materials[m].tex_name << std::endl; + fs << "# Faces" << std::endl; + } + for (size_t i = 0; i < tex_mesh.tex_polygons[m].size(); ++i) + { + // Write faces with "f" + fs << "f"; + size_t j = 0; + // There's one UV per vertex per face, i.e., the same vertex can have + // different UV depending on the face. + for (j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) + { + unsigned int idx = tex_mesh.tex_polygons[m][i].vertices[j] + 1; + fs << " " << idx + << "/" << 3*(i+f_idx) +j+1; + //<< "/" << idx; // vertex index in obj file format starting with 1 + } + fs << std::endl; + } + //PCL_INFO ("%d faces in mesh %d \n", tex_mesh.tex_polygons[m].size () , m); + fs << "# "<< tex_mesh.tex_polygons[m].size() << " faces in mesh " << m << std::endl; + } + fs << "# End of File"; + + // Close obj file + //PCL_INFO ("Closing obj file\n"); + fs.close (); + + /* Write material defination for OBJ file*/ + // Open file + //PCL_INFO ("Writing material files\n"); + //dont do it if no material to write + if(tex_mesh.tex_materials.size() ==0) + return (0); + + std::ofstream m_fs; + m_fs.precision (precision); + m_fs.open (mtl_file_name.c_str ()); + //std::cout << "MTL file is located at_ " << mtl_file_name << std::endl; + // default + m_fs << "#" << std::endl; + m_fs << "# Wavefront material file" << std::endl; + m_fs << "#" << std::endl; + for(int m = 0; m < nr_meshes; ++m) + { + m_fs << "newmtl " << tex_mesh.tex_materials[m].tex_name << std::endl; + m_fs << "Ka "<< tex_mesh.tex_materials[m].tex_Ka.r << " " << tex_mesh.tex_materials[m].tex_Ka.g << " " << tex_mesh.tex_materials[m].tex_Ka.b << std::endl; // defines the ambient color of the material to be (r,g,b). + m_fs << "Kd "<< tex_mesh.tex_materials[m].tex_Kd.r << " " << tex_mesh.tex_materials[m].tex_Kd.g << " " << tex_mesh.tex_materials[m].tex_Kd.b << std::endl; // defines the diffuse color of the material to be (r,g,b). + m_fs << "Ks "<< tex_mesh.tex_materials[m].tex_Ks.r << " " << tex_mesh.tex_materials[m].tex_Ks.g << " " << tex_mesh.tex_materials[m].tex_Ks.b << std::endl; // defines the specular color of the material to be (r,g,b). This color shows up in highlights. + m_fs << "d " << tex_mesh.tex_materials[m].tex_d << std::endl; // defines the transparency of the material to be alpha. + m_fs << "Ns "<< tex_mesh.tex_materials[m].tex_Ns << std::endl; // defines the shininess of the material to be s. + m_fs << "illum "<< tex_mesh.tex_materials[m].tex_illum << std::endl; // denotes the illumination model used by the material. + // illum = 1 indicates a flat material with no specular highlights, so the value of Ks is not used. + // illum = 2 denotes the presence of specular highlights, and so a specification for Ks is required. + m_fs << "map_Kd " << tex_mesh.tex_materials[m].tex_file << std::endl; + m_fs << "###" << std::endl; + } + m_fs.close (); + return (0); +} + +bool getPixelCoordinates(const pcl::PointXYZ &pt, const pcl::TextureMapping::Camera &cam, pcl::PointXY &UV_coordinates) +{ + if (pt.z > 0) + { + // compute image center and dimension + double sizeX = cam.width; + double sizeY = cam.height; + double cx, cy; + if (cam.center_w > 0) + cx = cam.center_w; + else + cx = sizeX / 2.0; + if (cam.center_h > 0) + cy = cam.center_h; + else + cy = sizeY / 2.0; + + double focal_x, focal_y; + if (cam.focal_length_w > 0) + focal_x = cam.focal_length_w; + else + focal_x = cam.focal_length; + if (cam.focal_length_h > 0) + focal_y = cam.focal_length_h; + else + focal_y = cam.focal_length; + + // project point on camera's image plane + UV_coordinates.x = static_cast ((focal_x * (pt.x / pt.z) + cx)); //horizontal + UV_coordinates.y = static_cast ((focal_y * (pt.y / pt.z) + cy)); //vertical + + // point is visible! + if (UV_coordinates.x >= 15.0 && UV_coordinates.x <= (sizeX - 15.0) && UV_coordinates.y >= 15.0 && UV_coordinates.y <= (sizeY - 15.0)) + { + return (true); // point was visible by the camera + } + } + + // point is NOT visible by the camera + UV_coordinates.x = -1.0f; + UV_coordinates.y = -1.0f; + return (false); // point was not visible by the camera +} + +bool isFaceProjected (const pcl::TextureMapping::Camera &camera, const pcl::PointXYZ &p1, const pcl::PointXYZ &p2, const pcl::PointXYZ &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3) +{ + return (getPixelCoordinates(p1, camera, proj1) && getPixelCoordinates(p2, camera, proj2) && getPixelCoordinates(p3, camera, proj3)); +} + +void getTriangleCircumscribedCircleCentroid( const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius) +{ + // compute centroid's coordinates (translate back to original coordinates) + circumcenter.x = static_cast (p1.x + p2.x + p3.x ) / 3; + circumcenter.y = static_cast (p1.y + p2.y + p3.y ) / 3; + double r1 = (circumcenter.x - p1.x) * (circumcenter.x - p1.x) + (circumcenter.y - p1.y) * (circumcenter.y - p1.y) ; + double r2 = (circumcenter.x - p2.x) * (circumcenter.x - p2.x) + (circumcenter.y - p2.y) * (circumcenter.y - p2.y) ; + double r3 = (circumcenter.x - p3.x) * (circumcenter.x - p3.x) + (circumcenter.y - p3.y) * (circumcenter.y - p3.y) ; + + // radius + radius = std::sqrt( std::max( r1, std::max( r2, r3) )) ; +} + +bool checkPointInsideTriangle(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt) +{ + // Compute vectors + Eigen::Vector2d v0, v1, v2; + v0(0) = p3.x - p1.x; v0(1) = p3.y - p1.y; // v0= C - A + v1(0) = p2.x - p1.x; v1(1) = p2.y - p1.y; // v1= B - A + v2(0) = pt.x - p1.x; v2(1) = pt.y - p1.y; // v2= P - A + + // Compute dot products + double dot00 = v0.dot(v0); // dot00 = dot(v0, v0) + double dot01 = v0.dot(v1); // dot01 = dot(v0, v1) + double dot02 = v0.dot(v2); // dot02 = dot(v0, v2) + double dot11 = v1.dot(v1); // dot11 = dot(v1, v1) + double dot12 = v1.dot(v2); // dot12 = dot(v1, v2) + + // Compute barycentric coordinates + double invDenom = 1.0 / (dot00*dot11 - dot01*dot01); + double u = (dot11*dot02 - dot01*dot12) * invDenom; + double v = (dot00*dot12 - dot01*dot02) * invDenom; + + // Check if point is in triangle + return ((u >= 0) && (v >= 0) && (u + v < 1)); +} diff --git a/odm_texturing/src/modifiedPclFunctions.hpp b/odm_texturing/src/modifiedPclFunctions.hpp new file mode 100644 index 000000000..5739df146 --- /dev/null +++ b/odm_texturing/src/modifiedPclFunctions.hpp @@ -0,0 +1,20 @@ +#pragma once + +// STL +#include +#include + +// PCL +#include +#include +#include + +int saveOBJFile(const std::string &file_name, const pcl::TextureMesh &tex_mesh, unsigned precision); + +bool getPixelCoordinates(const pcl::PointXYZ &pt, const pcl::TextureMapping::Camera &cam, pcl::PointXY &UV_coordinates); + +bool isFaceProjected (const pcl::TextureMapping::Camera &camera, const pcl::PointXYZ &p1, const pcl::PointXYZ &p2, const pcl::PointXYZ &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3); + +void getTriangleCircumscribedCircleCentroid(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius); + +bool checkPointInsideTriangle(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt); diff --git a/run.pl b/run.pl index 485c977dc..5603bfefb 100755 --- a/run.pl +++ b/run.pl @@ -67,14 +67,14 @@ sub parseArgs { ## defaults $args{"--match-size"} = "200"; - $args{"--resize-to"} = "3000"; + $args{"--resize-to"} = "1200"; $args{"--start-with"} = "resize"; - $args{"--end-with"} = "pmvs"; + $args{"--end-with"} = "odm_texturing"; $args{"--cmvs-maxImages"} = 100; - $args{"--matcher-ratio"} = 0.6; + $args{"--matcher-ratio"} = 0.6; $args{"--matcher-threshold"} = 2.0; $args{"--pmvs-level"} = 1; @@ -82,6 +82,14 @@ sub parseArgs { $args{"--pmvs-threshold"} = 0.7; $args{"--pmvs-wsize"} = 7; $args{"--pmvs-minImageNum"} = 3; + + $args{"--odm_meshing-maxVertexCount"} = 100000; + $args{"--odm_meshing-octreeDepth"} = 9; + $args{"--odm_meshing-samplesPerNode"} = 1; + $args{"--odm_meshing-solverDivide"} = 9; + + $args{"--odm_texturing-textureResolution"} = 4096; + $args{"--odm_texturing-textureWithSize"} = 3600; for($i = 0; $i <= $#ARGV; $i++) { if($ARGV[$i] =~ /^--[^a-z\-]*/){ @@ -188,13 +196,55 @@ sub parseArgs { die "\n invalid parameter for \"".$ARGV[$i]."\": ".$ARGV[$i+1]; } } + if($ARGV[$i] eq "--odm_meshing-maxVertexCount"){ + if($ARGV[$i+1] =~ /^[0-9]*$/){ + $args{$ARGV[$i]} = $ARGV[$i+1]; + } else { + die "\n invalid parameter for \"".$ARGV[$i]."\": ".$ARGV[$i+1]; + } + } + if($ARGV[$i] eq "--odm_meshing-octreeDepth"){ + if($ARGV[$i+1] =~ /^[0-9]*$/){ + $args{$ARGV[$i]} = $ARGV[$i+1]; + } else { + die "\n invalid parameter for \"".$ARGV[$i]."\": ".$ARGV[$i+1]; + } + } + if($ARGV[$i] eq "--odm_meshing-samplesPerNode"){ + if($ARGV[$i+1] =~ /^[0-9]*\.?[0-9]*$/){ + $args{$ARGV[$i]} = $ARGV[$i+1]; + } else { + die "\n invalid parameter for \"".$ARGV[$i]."\": ".$ARGV[$i+1]; + } + } + if($ARGV[$i] eq "--odm_meshing-solverDivide"){ + if($ARGV[$i+1] =~ /^[0-9]*$/){ + $args{$ARGV[$i]} = $ARGV[$i+1]; + } else { + die "\n invalid parameter for \"".$ARGV[$i]."\": ".$ARGV[$i+1]; + } + } + if($ARGV[$i] eq "--odm_texturing-textureResolution"){ + if($ARGV[$i+1] =~ /^[0-9]*$/){ + $args{$ARGV[$i]} = $ARGV[$i+1]; + } else { + die "\n invalid parameter for \"".$ARGV[$i]."\": ".$ARGV[$i+1]; + } + } + if($ARGV[$i] eq "--odm_texturing-textureWithSize"){ + if($ARGV[$i+1] =~ /^[0-9]*$/){ + $args{$ARGV[$i]} = $ARGV[$i+1]; + } else { + die "\n invalid parameter for \"".$ARGV[$i]."\": ".$ARGV[$i+1]; + } + } } } } if($args{"--help"}){ - print "\nusage: run.pl [options]"; - print "\nit should be run from the folder containing the images to be reconstructed"; + print "\nusgae: run.pl [options]"; + print "\nit should be run from the folder contining the images to which should reconstructed"; print "\n"; print "\noptions:"; print "\n --help: "; @@ -202,7 +252,7 @@ sub parseArgs { print "\n "; print "\n --resize-to: "; - print "\n default: 3000"; + print "\n default: 1200"; print "\n will resize the images so that the maximum width/height of the images are smaller or equal to the specified number"; print "\n if \"--resize-to orig\" is used it will use the images without resizing"; print "\n "; @@ -232,7 +282,7 @@ sub parseArgs { print "\n --matcher-threshold: (percent)"; print "\n default: 2.0"; - print "\n ignore matched keypoints if the two images share less than percent of keypoints"; + print "\n ignore matched keypoints if the two images share less then percent of keypoints"; print "\n "; print "\n --matcher-ratio: "; + print "\n default: 100000"; + print "\n The maximum vertex count of the output mesh."; + + print "\n --odm_meshing-octreeDepth: "; + print "\n default: 9"; + print "\n Octree depth used in the mesh reconstruction, increase to get more vertices, recommended values are 8-12"; + + print "\n --odm_meshing-samplesPerNode: "; + print "\n default: 1"; + print "\n Number of points per octree node, recommended value: 1.0"; + exit; } @@ -452,14 +514,14 @@ sub getKeypoints { if($fileObject->{isOk}){ if($args{"--lowe-sift"}){ - $vlsiftJobs .= "echo -n \" $c/$objectStats{good} - \" && convert -format pgm \"$fileObject->{step_0_resizedImage}\" \"$fileObject->{step_1_pgmFile}\""; + $vlsiftJobs .= "echo -n \"$c/$objectStats{good} - \" && convert -format pgm \"$fileObject->{step_0_resizedImage}\" \"$fileObject->{step_1_pgmFile}\""; $vlsiftJobs .= " && \"$BIN_PATH/sift\" < \"$fileObject->{step_1_pgmFile}\" > \"$fileObject->{step_1_keyFile}\""; $vlsiftJobs .= " && gzip -f \"$fileObject->{step_1_keyFile}\""; $vlsiftJobs .= " && rm -f \"$fileObject->{step_1_pgmFile}\""; $vlsiftJobs .= " && rm -f \"$fileObject->{step_1_keyFile}.sift\"\n"; } else { unless (-e "$jobOptions{jobDir}/$fileObject->{base}.key.bin") { - $vlsiftJobs .= "echo -n \" $c/$objectStats{good} - \" && convert -format pgm \"$fileObject->{step_0_resizedImage}\" \"$fileObject->{step_1_pgmFile}\""; + $vlsiftJobs .= "echo -n \"$c/$objectStats{good} - \" && convert -format pgm \"$fileObject->{step_0_resizedImage}\" \"$fileObject->{step_1_pgmFile}\""; $vlsiftJobs .= " && \"$BIN_PATH/vlsift\" \"$fileObject->{step_1_pgmFile}\" -o \"$fileObject->{step_1_keyFile}.sift\" > /dev/null && perl \"$BIN_PATH/../convert_vlsift_to_lowesift.pl\" \"$jobOptions{jobDir}/$fileObject->{base}\""; $vlsiftJobs .= " && gzip -f \"$fileObject->{step_1_keyFile}\""; $vlsiftJobs .= " && rm -f \"$fileObject->{step_1_pgmFile}\""; @@ -575,7 +637,7 @@ sub bundler { $bundlerOptions .= "--variable_focal_length\n"; $bundlerOptions .= "--use_focal_estimate\n"; $bundlerOptions .= "--constrain_focal\n"; - $bundlerOptions .= "--constrain_focal_weight 0.01\n"; + $bundlerOptions .= "--constrain_focal_weight 0.0\n"; $bundlerOptions .= "--estimate_distortion\n"; $bundlerOptions .= "--run_bundle"; @@ -636,6 +698,76 @@ sub pmvs { run("\"$BIN_PATH/pmvs2\" pmvs/ option-0000"); system("cp -Rf \"$jobOptions{jobDir}/pmvs/models\" \"$jobOptions{jobDir}-results\""); + + if($args{"--end-with"} ne "pmvs"){ + odm_meshing(); + } +} + +sub odm_meshing { + print "\n"; + print "\n - running meshing - "; + print "\n"; + + chdir($jobOptions{jobDir}); + mkdir($jobOptions{jobDir}."/odm_meshing"); + + + run("\"$BIN_PATH/odm_meshing\" -inputFile $jobOptions{jobDir}-results/option-0000.ply -outputFile $jobOptions{jobDir}-results/odm_mesh-0000.ply -logFile $jobOptions{jobDir}/odm_meshing/odm_meshing_log.txt -maxVertexCount $args{'--odm_meshing-maxVertexCount'} -octreeDepth $args{'--odm_meshing-octreeDepth'} -samplesPerNode $args{'--odm_meshing-samplesPerNode'}" ); + + if($args{"--end-with"} ne "odm_meshing"){ + odm_texturing(); + } +} + +sub odm_texturing { + print "\n"; + print "\n - running texturing - "; + print "\n"; + + chdir($jobOptions{jobDir}); + mkdir($jobOptions{jobDir}."/odm_texturing"); + mkdir("$jobOptions{jobDir}-results/odm_texturing"); + + + run("\"$BIN_PATH/odm_texturing\" -bundleFile $jobOptions{jobDir}/pmvs/bundle.rd.out -imagesPath $jobOptions{srcDir}/ -imagesListPath $jobOptions{jobDir}/pmvs/list.rd.txt -inputModelPath $jobOptions{jobDir}-results/odm_mesh-0000.ply -outputFolder $jobOptions{jobDir}-results/odm_texturing/ -textureResolution $args{'--odm_texturing-textureResolution'} -bundleResizedTo $jobOptions{resizeTo} -textureWithSize $args{'--odm_texturing-textureWithSize'} -logFile $jobOptions{jobDir}/odm_texturing/odm_texturing_log.txt" ); + + if($args{"--end-with"} ne "odm_texturing"){ + odm_georeferencing(); + } + +} + +sub odm_georeferencing { + print "\n"; + print "\n - running georeferencing - "; + print "\n"; + + chdir($jobOptions{jobDir}); + mkdir($jobOptions{jobDir}."/odm_georeferencing"); + + run("\"$BIN_PATH/odm_extract_utm\" -imagesPath $jobOptions{srcDir}/ -imageListFile $jobOptions{jobDir}/pmvs/list.rd.txt -outputCoordFile $jobOptions{jobDir}/odm_georeferencing/coordFile.txt"); + + run("\"$BIN_PATH/odm_georef\" -bundleFile $jobOptions{jobDir}/pmvs/bundle.rd.out -coordFile $jobOptions{jobDir}/odm_georeferencing/coordFile.txt -inputFile $jobOptions{jobDir}-results/odm_texturing/odm_textured_model.obj -outputFile $jobOptions{jobDir}-results/odm_texturing/odm_textured_model_geo.obj"); + + if($args{"--end-with"} ne "odm_georeferencing"){ + odm_orthophoto(); + } + +} + + +sub odm_orthophoto { + print "\n"; + print "\n - running orthophoto generation - "; + print "\n"; + + chdir($jobOptions{jobDir}); + mkdir($jobOptions{jobDir}."/odm_orthophoto"); + + + run("\"$BIN_PATH/odm_orthophoto\" -inputFile $jobOptions{jobDir}-results/odm_texturing/odm_textured_model_geo.obj -outputFile $jobOptions{jobDir}-results/odm_orthphoto.png -resolution 20.0 -boundry -200 -200 -200 200 200 200 200 -200"); + } parseArgs(); @@ -644,12 +776,16 @@ sub pmvs { chdir($jobOptions{jobDir}); switch ($args{"--start-with"}) { - case "resize" { resize(); } - case "getKeypoints" { getKeypoints(); } - case "match" { match(); } - case "bundler" { bundler(); } - case "cmvs" { cmvs(); } - case "pmvs" { pmvs(); } + case "resize" { resize(); } + case "getKeypoints" { getKeypoints(); } + case "match" { match(); } + case "bundler" { bundler(); } + case "cmvs" { cmvs(); } + case "pmvs" { pmvs(); } + case "odm_meshing" { odm_meshing(); } + case "odm_texturing" { odm_texturing(); } + case "odm_georeferencing" { odm_georeferencing(); } + case "odm_orthophoto" { odm_orthophoto(); } } print "\n";