From a602c1d09a1ded9f01d329940c4c6a75ee825bb9 Mon Sep 17 00:00:00 2001 From: ThomasD Date: Tue, 15 Mar 2022 16:33:08 +0100 Subject: [PATCH 1/4] add stereopolis capture --- CMakeLists.txt | 32 ++- include/cml/capture/StereopolisCapture.h | 132 +++-------- include/cml/capture/ZipTiffCapture.h | 160 +++++++++++++ include/cml/image/Array2D.h | 23 +- src/CMakeLists.txt | 10 +- src/cml/capture/StereopolisCapture.cpp | 165 ++++++++++++++ src/cml/image/Array2D.cpp | 278 ++++++++++++++++++++++- src/cml/slam/modslam.cpp | 104 ++++++--- 8 files changed, 747 insertions(+), 157 deletions(-) create mode 100644 include/cml/capture/ZipTiffCapture.h create mode 100644 src/cml/capture/StereopolisCapture.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e7698c19..ba7a3d23 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ project(CML set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) +set(CMAKE_PREFIX_PATH "/home/tdaumain/Qt/6.2.1/gcc_64") set_property(GLOBAL PROPERTY USE_FOLDERS ON) set_property(GLOBAL PROPERTY ALLOW_DUPLICATE_CUSTOM_TARGETS 1) @@ -81,11 +82,13 @@ endif() add_subdirectory(thirdparty/g2o) + if (ANDROID) set(AVFORMAT_FOUND 0) set(AVCODEC_FOUND 0) set(AVUTIL_FOUND 0) set(SWSCALE_FOUND 0) + set(TURBOJPEG_FOUND 0) else() find_package(PkgConfig QUIET) if (PKG_CONFIG_FOUND) @@ -101,11 +104,18 @@ if (PKG_CONFIG_FOUND) set(AVUTIL_FOUND 0) set(SWSCALE_FOUND 0) endif() + option(ENABLE_TURBOJPEG "Enable TurboJPEG" ${ENABLE_DEFAULT_EXTERNAL_LIB}) + if (ENABLE_TURBOJPEG) + pkg_check_modules(TURBOJPEG REQUIRED "libturbojpeg") + else() + set(TURBOJPEG_FOUND 0) + endif() else() set(AVFORMAT_FOUND 0) set(AVCODEC_FOUND 0) set(AVUTIL_FOUND 0) set(SWSCALE_FOUND 0) + set(TURBOJPEG_FOUND 0) endif() endif() @@ -134,7 +144,7 @@ endif() # endif() -option(ENABLE_OPENCV "Enable OpenCV (Version 4)" OFF) +option(ENABLE_OPENCV "Enable OpenCV (Version 4)" ${ENABLE_DEFAULT_EXTERNAL_LIB}) if (ENABLE_OPENCV) find_package(OpenCV 4.0 REQUIRED) endif() @@ -212,6 +222,7 @@ set(CML_HAVE_CERES ${CERES_FOUND}) set(CML_HAVE_G2O ${G2O_FOUND}) set(CML_HAVE_YAML_CPP ${Yaml-cpp_FOUND}) set(CML_HAVE_OPENCV ${OPENCV_FOUND}) +set(CML_HAVE_TURBOJPEG ${TURBOJPEG_FOUND}) set(CML_USE_OPENMP ${USE_OPENMP}) set(CML_ENABLE_GUI ${ENABLE_GUI}) set(CML_USE_GOOGLE_HASH ${USE_GOOGLE_HASH}) @@ -237,26 +248,23 @@ add_subdirectory(src) target_compile_options(CML PUBLIC ${COMPILER_FLAG_FPIC}) -#if (NOT ANDROID) -# -#endif() -#target_compile_definitions(CML PUBLIC -DEIGEN_MAX_ALIGN_BYTES=64) -#target_compile_definitions(CML PUBLIC -DENABLE_SSE=1) +if (NOT ANDROID) +target_compile_options(CML PUBLIC -march=native) +endif() +target_compile_definitions(CML PUBLIC -DEIGEN_MAX_ALIGN_BYTES=64) +target_compile_definitions(CML PUBLIC -DENABLE_SSE=1) #target_compile_definitions(CML PUBLIC -DEIGEN_USE_MKL_ALL=1) # target_compile_options(CML PUBLIC -Wall -Werror -Wuninitialized) - +target_compile_options(CML PUBLIC -flto) +target_link_options(CML PUBLIC -flto -fopenmp) # target_link_options(CML PUBLIC -mllvm) -target_compile_options(CML PUBLIC ${OpenMP_CXX_FLAGS} -static) -target_link_options(CML PUBLIC ${OpenMP_CXX_FLAGS} -static) - - if (CMAKE_BUILD_TYPE STREQUAL "Debug") target_compile_definitions(CML PUBLIC -DDEBUG=1) target_compile_options(CML PUBLIC -g) elseif (CMAKE_BUILD_TYPE STREQUAL "Release") - target_compile_options(CML PUBLIC -O2 -fno-math-errno) + target_compile_options(CML PUBLIC -O3 -fno-math-errno) elseif (CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo") target_compile_options(CML PUBLIC -Og -fno-math-errno -g) target_compile_definitions(CML PUBLIC -DNDEBUG=1) diff --git a/include/cml/capture/StereopolisCapture.h b/include/cml/capture/StereopolisCapture.h index 3fe28752..8f43f0ad 100644 --- a/include/cml/capture/StereopolisCapture.h +++ b/include/cml/capture/StereopolisCapture.h @@ -1,114 +1,50 @@ -#ifndef CML_STEREOPOLISCAPTURE_H -#define CML_STEREOPOLISCAPTURE_H +// +// Created by tbelos on 16/05/19. +// -#include "cml/config.h" +#ifndef CML_StereopolisCapture_H +#define CML_StereopolisCapture_H -#if CML_HAVE_LIBZIP +#include +#include +#include -#include "ZipCaptureHelper.h" +#include "cml/config.h" +#include "AbstractCapture.h" +#include "cml/image/LookupTable.h" namespace CML { - class StereopolisCapture : public AbstractMultithreadFiniteCapture, public ZipCaptureHelper { + class StereopolisCapture : public AbstractMultithreadFiniteCapture { public: - inline StereopolisCapture(const std::string &zipPath) { - loadZip(zipPath, ".tif"); - - uint8_t *data; - size_t size; - decompressFile(1, &data, &size); - auto images = loadTiffImage(data, size); - mMask = loadPngImage(zipPath + ".mask.png").first.castToUChar(); - - int histogram[256]; - for (int i = 0; i < 256; i++) { - histogram[i] = 0; - } - for (int y = 0; y < images.first.getHeight(); y++) { - for (int x = 0; x < images.first.getWidth(); x++) { - histogram[(int)images.first(x,y)]+=1; - } - } - int threshold = (images.first.getWidth() * images.first.getHeight()) * 0.999; - for (int i = 1; i < 256; i++) { - histogram[i] += histogram[i - 1]; - if (histogram[i] > threshold) { - mImageMax = i; - break; - } - } - - mLookupTable = GrayLookupTable::exp(255, 1.005f); - - images.first = images.first * (255.0f / mImageMax); - - int top = 0, bottom = images.first.getHeight() - 1; - - for (int y = 0; y < images.first.getHeight(); y++) { - for (int x = 0; x < images.first.getWidth(); x++) { - if (mMask(x,y) < 128 || images.first(x,y) > 255.9) { - if (y < images.first.getHeight() / 2) { - top = std::max(top, y); - } else { - bottom = std::min(bottom, y); - } - } - } - } + StereopolisCapture() = default; + StereopolisCapture(const std::string &path, const bool& reverse, const int & start, const double& expFactor, const int& topFactor ); + ~StereopolisCapture(); + bool isInit(); + void createPathList(std::string path); + void createPathListAll(const std::string &path); - mCaptureImageGenerator = new CaptureImageGenerator(images.first.getWidth(), (bottom - top)); + Ptr multithreadNext() final; - mCameraParameters = parseInternalStereopolisCalibration(zipPath + ".xml", mCaptureImageGenerator->getOutputSize(), top, bottom); - - - - } - - inline int remaining() final { - return imageNumbers() - mCurrentImage; - } - - inline int imageNumbers() final { - return getImageNumber(); - } + int remaining() final; protected: - inline Ptr multithreadNext() { - uint8_t *data; - size_t size; - decompressFile(mCurrentImage, &data, &size); - auto images = loadTiffImage(data, size); - images.first = images.first * (255.0f / mImageMax); - - for (int y = 0; y < images.first.getHeight(); y++) { - for (int x = 0; x < images.first.getWidth(); x++) { - if (mMask(x,y) < 128) { - images.first(x,y) = std::numeric_limits::quiet_NaN(); - images.second(x,y) = ColorRGBA(0,0,0,0); - } - /* else if (images.first(x,y) > 255.9) { - images.first(x,y) = std::numeric_limits::quiet_NaN(); - images.second(x,y) = ColorRGBA(0,0,0,0); - } */ - } - } - - CaptureImageMaker imageMaker = mCaptureImageGenerator->create(); - imageMaker.setImage(images.first) - .setImage(images.second) - .setPath(getFilename(mCurrentImage)) - .setTime((scalar_t)mCurrentImage / 10.0) - .setCalibration(mCameraParameters) - .setLut(&mLookupTable); - - mCurrentImage++; - - return imageMaker.generate(); - } + FloatImage loadImage(int id); private: + bool mIsInit = false; + std::string mPath; + bool mReverse = 0 ; + int mStart = 0 ; + std::vector mPathList; + std::vector mTimestamps; + + float mTime = 0; + size_t mCurrentIndex; + int mWidth, mHeight; + CaptureImageGenerator *mCaptureImageGenerator; InternalCalibration *mCameraParameters; int mCurrentImage = 1; @@ -116,11 +52,9 @@ namespace CML { GrayLookupTable mLookupTable; float mImageMax = 0; - }; } -#endif -#endif \ No newline at end of file +#endif //CML_StereopolisCapture_H diff --git a/include/cml/capture/ZipTiffCapture.h b/include/cml/capture/ZipTiffCapture.h new file mode 100644 index 00000000..1a2efc9c --- /dev/null +++ b/include/cml/capture/ZipTiffCapture.h @@ -0,0 +1,160 @@ +#ifndef CML_ZIPTIFFCAPTURE_H +#define CML_ZIPTIFFCAPTURE_H + +#include "cml/config.h" + +#if CML_HAVE_LIBZIP + +#include "ZipCaptureHelper.h" + + +namespace CML { + + class ZipTiffCapture : public AbstractMultithreadFiniteCapture, public ZipCaptureHelper { + + public: + inline ZipTiffCapture(const std::string &zipPath) { + logger.important("open zip file"); + loadZip(zipPath, ".tif"); + + uint8_t *data; + size_t size; + decompressFile(1, &data, &size); + logger.important("load tiff image"); + auto images = loadTiffImage(data, size); + logger.important("load mask"); + mMask = loadPngImage(zipPath + ".mask.png").first.castToUChar(); + logger.important("mask loaded"); + mMask.saveBmp("/home/tdaumain/mask.bmp"); + + //Add the real Time of the images + // Parse Time file + logger.important("Parsing time file..."); + std::ifstream timesFile; + timesFile.open((zipPath + ".times.txt").c_str()); + if (timesFile.is_open()) + { + while(!timesFile.eof() && timesFile.good()) + { + char buf[1000]; + timesFile.getline(buf, 1000); + int id; + double stamp; + if(2 == sscanf(buf, "%d %lf", &id, &stamp)) + { + mTimestamps.push_back(stamp); + } + + } + timesFile.close(); + } else { + throw std::runtime_error("Missing times.txt for STEREOPOLIS Dataset"); + } + + + int histogram[256]; + for (int i = 0; i < 256; i++) { + histogram[i] = 0; + } + for (int y = 0; y < images.first.getHeight(); y++) { + for (int x = 0; x < images.first.getWidth(); x++) { + histogram[(int)images.first(x,y)]+=1; + } + } + int threshold = (images.first.getWidth() * images.first.getHeight()) * 0.999; + for (int i = 1; i < 256; i++) { + histogram[i] += histogram[i - 1]; + if (histogram[i] > threshold) { + mImageMax = i; + break; + } + } + + mLookupTable = GrayLookupTable::exp(255, 1.005f); + + images.first = images.first * (255.0f / mImageMax); + + //crop with mask + int top = 0, bottom = images.first.getHeight() - 1; + + for (int y = 0; y < images.first.getHeight(); y++) { + for (int x = 0; x < images.first.getWidth(); x++) { + if (mMask(x,y) < 128 || images.first(x,y) > 255.9) { + if (y < images.first.getHeight() / 2) { + top = std::max(top, y); + } else { + bottom = std::min(bottom, y); + } + } + } + } + logger.important("TOP = " + std::to_string(top) + " BOTTOM = " + std::to_string(bottom)); + + + logger.important("create captureimagegenrator"); + mCaptureImageGenerator = new CaptureImageGenerator(images.first.getWidth(), (bottom - top)); + + logger.important("Internal Calibration"); + mCameraParameters = parseInternalStereopolisCalibration(zipPath + ".xml", mCaptureImageGenerator->getOutputSize(), top, bottom); + + + logger.important("ZipTiffCapture created"); + + } + + inline int remaining() final { + return imageNumbers() - mCurrentImage; + } + + inline int imageNumbers() final { + return getImageNumber(); + } + + protected: + inline Ptr multithreadNext() { + uint8_t *data; + size_t size; + decompressFile(mCurrentImage, &data, &size); + auto images = loadTiffImage(data, size); + + images.first = images.first * (255.0f / mImageMax); + + for (int y = 0; y < images.first.getHeight(); y++) { + for (int x = 0; x < images.first.getWidth(); x++) { + if (mMask(x,y) < 128) { + images.first(x,y) = std::numeric_limits::quiet_NaN(); + images.second(x,y) = ColorRGBA(0,0,0,0); + } + } + } + + CaptureImageMaker imageMaker = mCaptureImageGenerator->create(); + imageMaker.setImage(images.first) + .setImage(images.second) + .setPath(getFilename(mCurrentImage)) + /*.setTime((scalar_t)mCurrentImage / 10.0)*/.setTime(mTimestamps[mCurrentImage]) + .setCalibration(mCameraParameters) + .setLut(&mLookupTable); + + mCurrentImage++; + + return imageMaker.generate(); + } + + private: + CaptureImageGenerator *mCaptureImageGenerator; + InternalCalibration *mCameraParameters; + int mCurrentImage = 1; + GrayImage mMask; + GrayLookupTable mLookupTable; + float mImageMax = 0; + std::vector mTimestamps; + + + }; + +} + +#endif + +#endif diff --git a/include/cml/image/Array2D.h b/include/cml/image/Array2D.h index 6770d17b..e1169969 100644 --- a/include/cml/image/Array2D.h +++ b/include/cml/image/Array2D.h @@ -351,7 +351,9 @@ namespace CML { } template void convolution(const Array2D &kernel, Array2D &newImage) const { -#pragma omp single + #if CML_USE_OPENMP + #pragma omp single + #endif { if (newImage.getWidth() != getWidth() || newImage.getHeight() != getHeight()) { newImage = Array2D(getWidth(), getHeight()); @@ -375,7 +377,9 @@ namespace CML { } } +#if CML_USE_OPENMP #pragma omp for schedule(static) +#endif for (int img_y = 0; img_y < getHeight() - kernel.getHeight(); img_y++) { int img_x; for (img_x = 0; img_x < (getWidth() - kernel.getWidth()) - 4; img_x = img_x + 4) { @@ -430,7 +434,9 @@ namespace CML { } +#if CML_USE_OPENMP #pragma omp for schedule(static) +#endif for (int img_y = -res_shifty; img_y < res_shifty; img_y++) { int img_x; for (img_x = -res_shiftx; img_x + res_shiftx < getWidth(); img_x++) { @@ -446,7 +452,9 @@ namespace CML { } } +#if CML_USE_OPENMP #pragma omp for schedule(static) +#endif for (int img_y = getHeight() - kernel.getHeight(); img_y < (getHeight() - res_shifty); img_y++) { int img_x; for (img_x = -res_shiftx; img_x + res_shiftx < getWidth(); img_x++) { @@ -499,7 +507,9 @@ namespace CML { template Array2D castToUChar() const { Array2D result(getWidth(), getHeight()); + #if CML_USE_OPENMP #pragma omp for schedule(static) + #endif for (int y = 0; y < result.getHeight(); y++) { for (int x = 0; x < result.getWidth(); x++) { result(x, y) = fastRound(get(x,y)); @@ -510,13 +520,17 @@ namespace CML { template void castToUChar(Array2D &result) const { + #if CML_USE_OPENMP #pragma omp single + #endif { if (result.getWidth() != getWidth() || result.getHeight() != getHeight()) { result = Array2D(getWidth(), getHeight()); } } + #if CML_USE_OPENMP #pragma omp for schedule(static) + #endif for (int y = 0; y < result.getHeight(); y++) { for (int x = 0; x < result.getWidth(); x++) { result(x, y) = fastRound(get(x,y)); @@ -530,7 +544,9 @@ namespace CML { return castToUChar(); } else { Array2D result(getWidth(), getHeight()); + #if CML_USE_OPENMP #pragma omp for + #endif for (int y = 0; y < result.getHeight(); y++) { for (int x = 0; x < result.getWidth(); x++) { result(x, y) = get(x,y); @@ -707,13 +723,14 @@ namespace CML { }; - Pair loadTiffImage(const uint8_t *data, size_t lenght); + Pair loadTiffImage(const std::string& path); + Pair loadTiffImage(const uint8_t *str, size_t lenght); Pair loadJpegImage(const uint8_t *str, size_t lenght); Pair loadPngImage(const std::string &path); - + GrayImage loadGrayImage(std::string fileName); } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4787039e..3f18b3b0 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -16,6 +16,7 @@ add_library(CML STATIC cml/capture/TartanairCapture.cpp cml/capture/Eth3DCapture.cpp cml/capture/RobotCarCapture.cpp + cml/capture/StereopolisCapture.cpp cml/features/corner/PixelSelector.cpp @@ -79,7 +80,7 @@ add_library(CML STATIC lodepng/lodepng.cpp ) -target_link_libraries(CML Threads::Threads ${EIGEN3_LIBRARY} ${SuiteSparse_LIBRARIES} TIFF::TIFF tiffxx gdcmjpeg8) +target_link_libraries(CML Threads::Threads ${EIGEN3_LIBRARY} ${SuiteSparse_LIBRARIES} TIFF::TIFF TIFF::CXX gdcmjpeg8) target_include_directories(CML PUBLIC ${EIGEN3_INCLUDE_DIR} ${SOPHUS_INCLUDE_DIR} /usr/include/suitesparse) if (USE_OPENMP) @@ -155,6 +156,10 @@ if (GLOG_FOUND) target_link_libraries(CML ${GLOG_LIBRARIES}) endif() +if (TURBOJPEG_FOUND) + target_link_libraries(CML ${TURBOJPEG_LIBRARIES}) +endif() + if (ENABLE_GUI) qt6_wrap_cpp(ModelViewerWidgetSRC ../include/cml/gui/widgets/ModelWidget.h) qt6_wrap_cpp(FunctionWidgetSRC ../include/cml/gui/widgets/FunctionWidget.h) @@ -188,9 +193,6 @@ if (ENABLE_GUI) ) target_link_libraries(CML Qt6::Core Qt6::Widgets Qt6::OpenGL Qt6::OpenGLWidgets Qt6::PrintSupport Qt6::Multimedia Qt6::Charts ${OPENGL_LIBRARIES}) - if (MINGW) - target_link_libraries(CML opengl32 glu32 mingw32) - endif() endif() diff --git a/src/cml/capture/StereopolisCapture.cpp b/src/cml/capture/StereopolisCapture.cpp new file mode 100644 index 00000000..d5e01791 --- /dev/null +++ b/src/cml/capture/StereopolisCapture.cpp @@ -0,0 +1,165 @@ +#include +#include "cml/capture/StereopolisCapture.h" + +#if CML_HAVE_LIBZIP + +#include "cml/map/InternalCalibration.h" + +#define USE_TURBOJPEG 0 + +#if USE_TURBOJPEG +extern "C" { + #include +}; +#endif +#include +#include +#include +#include + +void CML::StereopolisCapture::createPathList(std::string path){ + for (const auto & entry : std::filesystem::directory_iterator(path)) + if(entry.path().extension() == ".tif" ) mPathList.push_back(entry.path()); + std::sort(mPathList.begin(), mPathList.end()); +} + +void CML::StereopolisCapture::createPathListAll(const std::string & path){ + std::string s1 = path + "/section_00"; + std::string s2 = path + "/section_01"; + for (const auto & entry : std::filesystem::directory_iterator(s1)) + if(entry.path().extension() == ".tif" ) mPathList.push_back(entry.path()); + for (const auto & entry : std::filesystem::directory_iterator(s2)) + if(entry.path().extension() == ".tif" ) mPathList.push_back(entry.path()); + std::sort(mPathList.begin(), mPathList.end()); + if(mReverse) + std::reverse(mPathList.begin(), mPathList.end()); + if(mStart > 0) + mPathList.erase(mPathList.begin(), mPathList.begin() + mStart); +} + +CML::StereopolisCapture::StereopolisCapture(const std::string& path, const bool& reverse, const int& start, const double& expFactor, const int& topFactor ){ + logger.important("create stereopolis capture"); + + mStart = start; + mReverse = reverse; + + //createPathList(path); + createPathListAll(path); + + std::string s = "number of files : " + std::to_string(mPathList.size()); + logger.important(s); + + // Parse Time file + logger.important("Parsing time file..."); + std::ifstream timesFile; + timesFile.open((path + "/times.txt").c_str()); + if (timesFile.is_open()) + { + while(!timesFile.eof() && timesFile.good()) + { + char buf[1000]; + timesFile.getline(buf, 1000); + int id; + double stamp; + if(2 == sscanf(buf, "%d %lf", &id, &stamp)) + { + mTimestamps.push_back(stamp); + } + + } + timesFile.close(); + } else { + throw std::runtime_error("Missing times.txt for STEREOPOLIS Dataset"); + } + + + if(reverse){ + std::reverse(mTimestamps.begin(), mTimestamps.end()); + logger.raw(std::to_string(mTimestamps[0]) + " " +std::to_string(mTimestamps[mTimestamps.size()])); + double d = mTimestamps[0]; + for (size_t i = 0; i < mTimestamps.size(); i++) { + mTimestamps[i] = d - mTimestamps[i]; + if(mTimestamps[i] < 0) throw std::runtime_error("bug in timestamp init"); + } + } + + if(start > 0){ + mTimestamps.erase(mTimestamps.begin(), mTimestamps.begin() + start); + } + + + logger.important("load tiff image"); + auto images = loadTiffImage(mPathList[0]); + logger.important("create mask"); + mMask = loadPngImage(path + "/mask.png").first.castToUChar(); + + mLookupTable = GrayLookupTable::exp(255, expFactor); // add at execution with -e (default= 1.005) + + int top = topFactor, bottom = images.first.getHeight() - 1; // add at execution with -k (default = 1000) + // int top = 436, bottom = 1275; + + for (int y = 0; y < images.first.getHeight(); y++) { + for (int x = 0; x < images.first.getWidth(); x++) { + if (mMask(x,y) < 128) { + if (y < images.first.getHeight() / 2) { + top = std::max(top, y); + } else { + bottom = std::min(bottom, y); + } + } + } + } + logger.important("TOP = " + std::to_string(top) + " BOTTOM = " + std::to_string(bottom)); + + logger.important("create capture image"); + mCaptureImageGenerator = new CaptureImageGenerator(images.first.getWidth(), (bottom - top)); + + + logger.important("create calib"); + mCameraParameters = parseInternalStereopolisCalibration(path + "/calib.xml", mCaptureImageGenerator->getOutputSize(), top, bottom); + + mCurrentIndex = 0; + + logger.info("STEREOPOLIS Capture " + path + " is open"); + +} + +CML::StereopolisCapture::~StereopolisCapture() { + delete mCaptureImageGenerator; + delete mCameraParameters; +} + +CML::Ptr CML::StereopolisCapture::multithreadNext() { + if (mCurrentIndex >= mPathList.size()) { + return Ptr(nullptr); + } + auto images = loadTiffImage(mPathList[mCurrentIndex]); + + // images.first = images.first * (255.0f / mImageMax); + for (int y = 0; y < images.first.getHeight(); y++) { + for (int x = 0; x < images.first.getWidth(); x++) { + if (mMask(x,y) < 128) { + images.first(x,y) = std::numeric_limits::quiet_NaN(); + images.second(x,y) = ColorRGBA(0,0,0,0); + } + } + } + CaptureImageMaker imageMaker = mCaptureImageGenerator->create(); + imageMaker.setImage(images.first) + .setImage(images.second) + .setPath(mPathList[mCurrentIndex]) + .setTime(mTimestamps[mCurrentIndex]) + .setCalibration(mCameraParameters) + .setLut(&mLookupTable) + ; + + mCurrentIndex++; + + return imageMaker.generate(); +} + +int CML::StereopolisCapture::remaining() { + return mPathList.size() - mCurrentIndex; +} + +#endif diff --git a/src/cml/image/Array2D.cpp b/src/cml/image/Array2D.cpp index 27ef299b..4b0b9dc1 100644 --- a/src/cml/image/Array2D.cpp +++ b/src/cml/image/Array2D.cpp @@ -10,10 +10,53 @@ #include #include "lodepng/lodepng.h" +#if CML_HAVE_AVFORMAT +extern "C" { +#include +#include +#include +#include +} +#define DEFAULT_RESIZE_ALGORITHM SWS_FAST_BILINEAR // Bicubic seems to be better than bilinear. Todo : try other algorithm +#endif + +#define CML_IMAGE_HORIZONTALFLIP false + namespace CML { Atomic __array2DCounter = 0; + #if CML_HAVE_AVFORMAT + + class FFMPEGContext { + + public: + FFMPEGContext() { + + } + + ~FFMPEGContext() { + if (mFrame != nullptr) { + av_frame_free(&mFrame); + } + if (mCodecCtx != nullptr) { + avcodec_close(mCodecCtx); + avcodec_free_context(&mCodecCtx); + } + if (mFormatCtx != nullptr) { + avformat_close_input(&mFormatCtx); + } + } + + AVFormatContext *mFormatCtx = nullptr; // ok + int mVideoStream = -1; + AVCodecContext *mCodecCtx = nullptr; // ok + AVFrame *mFrame = nullptr; // ok + AVCodec *mCodec = nullptr; + AVCodecParameters *mCodecParameters = nullptr; + }; + + #endif } @@ -92,7 +135,10 @@ CML::Array2D CML::Array2D::resize(int newWidth, in template<> void CML::Array2D::resize(int newWidth, int newHeight, Array2D &result) const { + +#if CML_USE_OPENMP #pragma omp single +#endif { if (result.getWidth() != newWidth || result.getHeight() != newHeight) { result = CML::Array2D(newWidth, newHeight); @@ -112,19 +158,115 @@ void CML::Array2D::resize(int newWidth, int newHeight, Array2D CML::loadTiffImage(const std::string& path) { + // std::istringstream input_TIFF_stream(std::string((char *)str, lenght)); + +//Populate input_TIFF_stream with TIFF image data +//... + // logger.important("opening the tif file : " + path +"\n"); + TIFF* tif = TIFFOpen(path.c_str(), "r"); + + // TIFF *tif = TIFFStreamOpen("MemTIFF", &input_TIFF_stream); + + if (!tif) { + std::cout << "Can't open the tiff file" << std::endl; + // return; + // throw std::runtime_error("Can't open the tiff file"); + } + + uint32_t width, height, bitspersample, depth; + + TIFFGetField(tif, TIFFTAG_IMAGELENGTH, &height); + TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, &width); + TIFFGetField(tif, TIFFTAG_BITSPERSAMPLE, &bitspersample); + //TIFFGetField(tif, TIFFTAG_IMAGEDEPTH, &depth); + depth = 3; + + FloatImage image(width, height); + Image colorImage(width, height); + + tdata_t buf = _TIFFmalloc(TIFFScanlineSize(tif)); + if (!buf) { + throw std::runtime_error("tiff can't malloc ?"); + } + uint8_t *buf8 = (uint8_t *) buf; + + uint32_t mask = 0; + for (uint32_t i = 0; i < bitspersample; i++) { + mask ^= 1U << i; + } + + float factor = 255.0f / (float) pow(2, bitspersample); + + for (uint32_t y = 0; y < height; y++) { + TIFFReadScanline(tif, buf, y); + uint32_t currentBitPosition = 0; + + for (uint32_t x = 0; x < width; x++) { + + float avg = 0; + + for (uint32_t c = 0; c < depth; c++) { + + uint32_t pos = currentBitPosition / 8; + uint32_t left = currentBitPosition % 8; + uint32_t right = (32 - bitspersample) - left; + + uint32_t *pintensity = (uint32_t *) &buf8[pos]; + uint32_t intensity = *pintensity; + + intensity = intensity >> right; + intensity = intensity & mask; + + float value = (float) intensity * factor; + + if (c == 0) { + colorImage(x, y).g() = value; + + } else if (c == 1) { + colorImage(x, y).b() = value; + + } else if (c == 2) { + colorImage(x, y).r() = value; + + } + + avg += value; + + currentBitPosition += bitspersample; + + } + + avg /= (float) depth; + + image(x, y) = avg; + + } + } + + + _TIFFfree(buf); + TIFFClose(tif); + + return {image, colorImage}; + +} + CML::Pair CML::loadTiffImage(const uint8_t *str, size_t lenght) { std::istringstream input_TIFF_stream(std::string((char *)str, lenght)); //Populate input_TIFF_stream with TIFF image data //... - TIFF *tif = TIFFStreamOpen("MemTIFF", &input_TIFF_stream); + TIFF *tif = nullptr; + tif = TIFFStreamOpen("MemTIFF", &input_TIFF_stream); if (!tif) { + // logger.important("Can't open the tif file : " + std::string((char *)str) + " !\n"); throw std::runtime_error("Can't open the tiff file"); } - uint32 width, height, bitspersample, depth; + uint32_t width, height, bitspersample, depth; TIFFGetField(tif, TIFFTAG_IMAGELENGTH, &height); TIFFGetField(tif, TIFFTAG_IMAGEWIDTH, &width); @@ -391,4 +533,134 @@ CML::Pair CML::loadPngImage(const std::string &path return {grayImage, colorImage}; -} \ No newline at end of file +} + +CML::GrayImage CML::loadGrayImage(std::string path) { + +#if CML_HAVE_AVFORMAT + logger.debug("Load image " + path); + + FFMPEGContext ctx; + + // Open video file + int errorCode = avformat_open_input(&ctx.mFormatCtx, path.c_str(), nullptr, nullptr); + if (errorCode != 0) { + throw std::runtime_error("Can't open '" + path + ". " ); + } + + // Retrieve stream information + if (avformat_find_stream_info(ctx.mFormatCtx, nullptr) < 0) { + throw std::runtime_error("Could not find stream information for '" + path + "'"); + } + + //av_dump_format(mFormatCtx, 0, path.c_str(), 0); + + // Find the first video stream + for (unsigned int i = 0; i < ctx.mFormatCtx->nb_streams; i++) { + AVCodecParameters *pLocalCodecParameters = ctx.mFormatCtx->streams[i]->codecpar; + AVCodec *pLocalCodec = avcodec_find_decoder(pLocalCodecParameters->codec_id); + + + if (pLocalCodecParameters->codec_type == AVMEDIA_TYPE_VIDEO) { + ctx.mVideoStream = i; + ctx.mCodec = pLocalCodec; + ctx.mCodecParameters = pLocalCodecParameters; + if (ctx.mCodec != nullptr) break; + } + } + + if (ctx.mVideoStream == -1) { + throw std::runtime_error("Could not find any compatible video stream for '" + path + "'"); + } + + + // Copy context + ctx.mCodecCtx = avcodec_alloc_context3(ctx.mCodec); + if (!ctx.mCodecCtx) { + throw std::runtime_error("Couldn't allocate codec context for '" + path + "'"); + } + + if (avcodec_parameters_to_context(ctx.mCodecCtx, ctx.mCodecParameters) < 0) { + fprintf(stderr, "Couldn't copy codec context"); + throw std::runtime_error("Couldn't copy codec context for '" + path + "'"); + } + + // Open codec + if (avcodec_open2(ctx.mCodecCtx, ctx.mCodec, nullptr) < 0) { + throw std::runtime_error("Couldn't open codec for '" + path + "'"); + } + + ctx.mFrame = av_frame_alloc(); + if (!ctx.mFrame) { + throw std::runtime_error("Couldn't allocate frame for '" + path + "'"); + } + + //mPacket = av_packet_alloc(); + //if (!mPacket) { + // throw std::runtime_error("Couldn't allocate packet for '" + path + "'"); + // } + + AVPacket packet; + + while (av_read_frame(ctx.mFormatCtx, &packet) >= 0) { + + if (packet.stream_index == ctx.mVideoStream) { + + int response = avcodec_send_packet(ctx.mCodecCtx, &packet); + if (response < 0) { + throw std::runtime_error("Error while sending a packet to the decoder"); + } + + while (response >= 0) { + + response = avcodec_receive_frame(ctx.mCodecCtx, ctx.mFrame); + if (response == AVERROR(EAGAIN) || response == AVERROR_EOF) { + break; + } else if (response < 0) { + // std::string error = av_err2str(response); + throw std::runtime_error("Error while receiving a frame from the decoder"); + } + + if (response >= 0) { + + // Create the frame + unsigned int width = ctx.mFrame->width; + unsigned int height = ctx.mFrame->height; + + GrayImage image(width, height); + uint8_t *const data[8] = {(uint8_t *) image.data(), nullptr, nullptr, nullptr, nullptr, nullptr, + nullptr}; + const int stride[8] = {(int) (image.getWidth() * sizeof(uint8_t)), 0, 0, 0, 0, 0, 0, 0}; + + SwsContext *sws_ctx = sws_getContext(width, height, (AVPixelFormat) ctx.mFrame->format, width, + height, AV_PIX_FMT_GRAY8, DEFAULT_RESIZE_ALGORITHM, NULL, NULL, + NULL); + sws_scale(sws_ctx, (uint8_t const *const *) ctx.mFrame->data, ctx.mFrame->linesize, 0, height, data, + stride); + sws_freeContext(sws_ctx); + + av_packet_unref(&packet); + + if (CML_IMAGE_HORIZONTALFLIP) { + return image.horizontalFlip(); + } else { + return image; + } + + } + + } + + } + + av_packet_unref(&packet); + + } + + throw std::runtime_error("The image file seems to be corrupted : " + path); +#else + throw std::runtime_error("No codec to decode the image. Please recompile with ffmpeg"); +#endif + + +} diff --git a/src/cml/slam/modslam.cpp b/src/cml/slam/modslam.cpp index ecedfce2..a6951804 100644 --- a/src/cml/slam/modslam.cpp +++ b/src/cml/slam/modslam.cpp @@ -20,7 +20,7 @@ #if CML_HAVE_LIBZIP #include -#include +#include #endif #include @@ -28,6 +28,7 @@ #include #include #include +#include #include @@ -48,45 +49,51 @@ Q_DECLARE_METATYPE(scalar_t) #endif #if !CML_IS_ANDROID -Ptr loadDataset(const std::string &path) { - -#if CML_HAVE_AVFORMAT -#if CML_ENABLE_GUI - if (path == "cam" || path == "webcam" || path == "camera") { - return new QtWebcamCapture(); - } -#endif +Ptr loadDataset(const std::string &path, const bool& reverse,const int& start, const double& expFactor, const int& topFactor ) { + +// #if CML_HAVE_AVFORMAT +// #if CML_ENABLE_GUI +// if (path == "cam" || path == "webcam" || path == "camera") { +// return new QtWebcamCapture(); +// } +// #endif +// +// try { +// Ptr capture = new CML::VideoCapture(path); +// return capture; +// } catch (const std::exception &e) { +// logger.error(e.what()); +// } +// #endif + +// #if CML_HAVE_LIBZIP +// try { +// Ptr capture = new CML::TUMCapture(path); +// return capture; +// } catch (const std::exception &e) { +// logger.error(e.what()); +// } try { - Ptr capture = new CML::VideoCapture(path); + Ptr capture = new CML::StereopolisCapture(path, reverse, start, expFactor, topFactor); return capture; } catch (const std::exception &e) { logger.error(e.what()); } -#endif -#if CML_HAVE_LIBZIP try { - Ptr capture = new CML::TUMCapture(path); + Ptr capture = new CML::ZipTiffCapture(path); return capture; } catch (const std::exception &e) { logger.error(e.what()); } - try { - Ptr capture = new CML::StereopolisCapture(path); - return capture; - } catch (const std::exception &e) { - logger.error(e.what()); - } -#endif - - try { - Ptr capture = new CML::KittyCapture(path); - return capture; - } catch (const std::exception &e) { - logger.error(e.what()); - } + // try { + // Ptr capture = new CML::KittyCapture(path); + // return capture; + // } catch (const std::exception &e) { + // logger.error(e.what()); + // } /* try { Ptr capture = new CML::EurocCapture(path); @@ -228,7 +235,7 @@ int main(int argc, char *argv[]) printTypeSize(); - // srand(29071996); + // srand(29031806); srand(time(nullptr)); #if CML_ENABLE_GUI @@ -247,12 +254,43 @@ int main(int argc, char *argv[]) std::string resultFormat = "all"; std::string saveImagePath = ""; - program.add_argument("-d", "--dataset").nargs(1).help("Path to the dataset").action([&capture](const std::string &value){ - capture = loadDataset(value); + int start = 0; + bool reverse = false; + double expF = 1.005; + int topF = 1000; + + program.add_argument("-i", "--reverse").nargs(0).help("active reverse mode").default_value(false).implicit_value(true); + + program.add_argument("-j", "--start").nargs(1).help("jump at frame n").action([&start](const std::string &value) { + if(!(1 == sscanf(value.c_str(), "%d", &start)) || start < 0 ){ + logger.error("start frame is wrong"); + } + logger.important("start is now : " + std::to_string(start)); + + }); + program.add_argument("-e", "--exp").nargs(1).help("get exp factor").action([&expF](const std::string &value) { + if(!(1 == sscanf(value.c_str(), "%lf", &expF)) || expF <= 1 ){ + logger.error("exp Factor is wrong"); + } + logger.important("exp factor is now : " + std::to_string(expF)); + + }); + + program.add_argument("-k", "--top").nargs(1).help("cut the images").action([&topF](const std::string &value) { + if(!(1 == sscanf(value.c_str(), "%d", &topF)) || topF < 0 ){ + logger.error("top is wrong"); + } + logger.important("top is now : " + std::to_string(topF)); + + }); + + program.add_argument("-d", "--dataset").nargs(1).help("Path to the dataset").action([&capture, &start, &topF, &expF, &program](const std::string &value){ + capture = loadDataset(value, program.get("--reverse"), start, expF, topF); if (capture.isNull()) { logger.error("Can't load dataset '" + value + "'"); } }); + //#if CML_ENABLE_GUI program.add_argument("-g", "--gui").nargs(0).help("Gui mode").default_value(true).implicit_value(true); program.add_argument("-t", "--terminal").nargs(0).help("Terminal mode").default_value(false).implicit_value(true); @@ -277,14 +315,8 @@ int main(int argc, char *argv[]) program.add_argument("-s", "--save").nargs(1).help("Save the images").action([&saveImagePath](const std::string &value) { saveImagePath = value; }); - program.add_argument("-v", "--verbose").nargs(0).help("Verbose").default_value(false).implicit_value(true); - program.parse_args(argc, argv); - if (program["--verbose"] == true) { - logger.setLogLevel(CML::MORE); - } - #if CML_ENABLE_GUI if (program["--gui"] == true) { executionMode = GUI; From 7bc189ed91c62a1a69491297d9fb034a1ef95812 Mon Sep 17 00:00:00 2001 From: ThomasD Date: Tue, 15 Mar 2022 16:40:04 +0100 Subject: [PATCH 2/4] corrected params(3) in internalCalibration (segfault in thread Hybrid) --- src/cml/capture/StereopolisCapture.cpp | 1 - src/cml/map/InternalCalibration.cpp | 15 ++++++++++----- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/cml/capture/StereopolisCapture.cpp b/src/cml/capture/StereopolisCapture.cpp index d5e01791..4b440d37 100644 --- a/src/cml/capture/StereopolisCapture.cpp +++ b/src/cml/capture/StereopolisCapture.cpp @@ -96,7 +96,6 @@ CML::StereopolisCapture::StereopolisCapture(const std::string& path, const bool& mLookupTable = GrayLookupTable::exp(255, expFactor); // add at execution with -e (default= 1.005) int top = topFactor, bottom = images.first.getHeight() - 1; // add at execution with -k (default = 1000) - // int top = 436, bottom = 1275; for (int y = 0; y < images.first.getHeight(); y++) { for (int x = 0; x < images.first.getWidth(); x++) { diff --git a/src/cml/map/InternalCalibration.cpp b/src/cml/map/InternalCalibration.cpp index ffbc4a54..782dd367 100644 --- a/src/cml/map/InternalCalibration.cpp +++ b/src/cml/map/InternalCalibration.cpp @@ -351,19 +351,24 @@ CML::InternalCalibration* CML::parseInternalStereopolisCalibration(std::string p if (modunif["TypeModele"][0]=="eModele_FishEye_10_5_5") { PinholeUndistorter pinhole{Vector2(f, f), Vector2(params[0], params[1])}; - FishEye10_5_5 *fishEye1055 = new FishEye10_5_5({params[2], params[3], params[4], params[5]}, {params[6], params[7]}, {params[8], params[9]}); + FishEye10_5_5 *fishEye1055 = new FishEye10_5_5({params[2], params[3], params[4], params[5], params[6]}, {params[7], params[8]}, {params[9], params[10]}); auto res = makeOptimalK_crop(pinhole, fishEye1055, size.cast(), outputSize); Vector4 params = res.first.getParameters(); float nonCroppedHeight = size.y() * outputSize.x() / size.x(); + // logger.important(" NON CROPPED : " + std::to_string(nonCroppedHeight)); + params(1) *= nonCroppedHeight / outputSize.y(); - top = top * outputSize.y() / size.y(); - bottom = bottom * outputSize.y() / size.y(); - params(3) = ((top + bottom) / 2) + top; + + params(3) = -nonCroppedHeight/2 + std::min((bottom - top), 480) + + 1.4f * (nonCroppedHeight - (bottom * nonCroppedHeight /size.y())); + + // logger.important("PARAMS = " + std::to_string(params(0)) + " " + std::to_string(params(1)) + " " + std::to_string(params(2)) + " " + std::to_string(params(3))); + PinholeUndistorter newpinhole(params); - return new InternalCalibration(pinhole, size.cast(), fishEye1055, params, outputSize.cast()); + return new InternalCalibration(pinhole, size.cast(), fishEye1055, newpinhole, outputSize.cast()); } else { From e63949135ebe438c5f77122e84415b96c5786a89 Mon Sep 17 00:00:00 2001 From: ThomasD Date: Tue, 29 Mar 2022 14:07:57 +0200 Subject: [PATCH 3/4] correct segfault - version to merge master -> ThomasD --- CMakeLists.txt | 13 +- include/cml/base/AbstractSlam.h | 18 +++ include/cml/capture/AbstractCapture.h | 2 + include/cml/capture/StereopolisCapture.h | 8 + include/cml/capture/ZipCaptureHelper.h | 54 ++++++- include/cml/capture/ZipStereopolisCapture.h | 115 ++++++++++++++ include/cml/capture/ZipTiffCapture.h | 160 -------------------- src/cml/base/AbstractSlam.cpp | 111 +++++++++++++- src/cml/slam/modslam.cpp | 14 +- src/cml/slam/modslam/Hybrid.cpp | 1 + 10 files changed, 321 insertions(+), 175 deletions(-) create mode 100644 include/cml/capture/ZipStereopolisCapture.h delete mode 100644 include/cml/capture/ZipTiffCapture.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ba7a3d23..f33dd258 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -249,17 +249,20 @@ add_subdirectory(src) target_compile_options(CML PUBLIC ${COMPILER_FLAG_FPIC}) if (NOT ANDROID) -target_compile_options(CML PUBLIC -march=native) +#target_compile_options(CML PUBLIC) endif() -target_compile_definitions(CML PUBLIC -DEIGEN_MAX_ALIGN_BYTES=64) -target_compile_definitions(CML PUBLIC -DENABLE_SSE=1) +# target_compile_definitions(CML PUBLIC -DEIGEN_MAX_ALIGN_BYTES=64) +# target_compile_definitions(CML PUBLIC -DENABLE_SSE=1) #target_compile_definitions(CML PUBLIC -DEIGEN_USE_MKL_ALL=1) # target_compile_options(CML PUBLIC -Wall -Werror -Wuninitialized) -target_compile_options(CML PUBLIC -flto) -target_link_options(CML PUBLIC -flto -fopenmp) +# target_compile_options(CML PUBLIC -flto) +# target_link_options(CML PUBLIC -flto -fopenmp) # target_link_options(CML PUBLIC -mllvm) +# target_compile_options(CML PUBLIC ${OpenMP_CXX_FLAGS} -static) +# target_link_options(CML PUBLIC ${OpenMP_CXX_FLAGS} -static) + if (CMAKE_BUILD_TYPE STREQUAL "Debug") target_compile_definitions(CML PUBLIC -DDEBUG=1) target_compile_options(CML PUBLIC -g) diff --git a/include/cml/base/AbstractSlam.h b/include/cml/base/AbstractSlam.h index 07408a2c..b24281db 100644 --- a/include/cml/base/AbstractSlam.h +++ b/include/cml/base/AbstractSlam.h @@ -64,6 +64,15 @@ namespace CML { Ptr getLastCaptureFrame(); + void addGroundtruth(std::string pathGroundtruth); + + Camera getGroundtruth(int index){ + if(!mHaveGroundtruth || index >= mGroundtruths.size()){ + throw("you tried to use groundtruth with slam without initialization"); + } + return mGroundtruths[index]; + } + virtual inline std::string getName() { return "Abstract Slam"; } @@ -153,6 +162,15 @@ namespace CML { GarbageCollectorInstance mGarbageCollectorInstance; + std::vector mTimes; + std::vector mGroundtruths; + + Vector3 correct_translation; + Matrix33 correct_rotation; + Camera correct_cam; + + bool mHaveGroundtruth = false; + int mMemoryLimit = 0; // MB Set mUnusedParameters; diff --git a/include/cml/capture/AbstractCapture.h b/include/cml/capture/AbstractCapture.h index 2f3cde17..d6594636 100644 --- a/include/cml/capture/AbstractCapture.h +++ b/include/cml/capture/AbstractCapture.h @@ -35,6 +35,8 @@ namespace CML { return false; } + virtual inline scalar_t getTime(){return 0;}; + }; class AbstractFiniteCapture : public AbstractCapture { diff --git a/include/cml/capture/StereopolisCapture.h b/include/cml/capture/StereopolisCapture.h index 8f43f0ad..615905f5 100644 --- a/include/cml/capture/StereopolisCapture.h +++ b/include/cml/capture/StereopolisCapture.h @@ -30,6 +30,14 @@ namespace CML { int remaining() final; + scalar_t getTime() final{ + return mTimestamps[mCurrentIndex]; + } + + inline int imageNumbers() final{ + return mCurrentIndex + mStart; + } + protected: FloatImage loadImage(int id); diff --git a/include/cml/capture/ZipCaptureHelper.h b/include/cml/capture/ZipCaptureHelper.h index b92917ae..f7bf457c 100644 --- a/include/cml/capture/ZipCaptureHelper.h +++ b/include/cml/capture/ZipCaptureHelper.h @@ -13,7 +13,11 @@ namespace CML { class ZipCaptureHelper { protected: - inline void loadZip(const std::string &filename, const std::string &suffix) { + inline void loadZip(const std::string &filename, const std::string &suffix, std::string extractPath = "") { + + mZipPath = filename; + mExtractPath = extractPath; + int ziperror = 0; mZipArchive = zip_open(filename.c_str(), ZIP_RDONLY, &ziperror); @@ -43,24 +47,65 @@ namespace CML { return loadJpegImage(data, size); } - inline void decompressFile(const std::string &filename, uint8_t **data, size_t *size) { + inline std::string decompressFile(const std::string &filename, uint8_t **data, size_t *size) { if (mZipBuffer.size() == 0) { mZipBuffer.resize(1e+8); } + FILE *f = nullptr; + std::string extractedFilePath = ""; + if (mExtractPath != "") { + + std::string codedFilename = filename; + std::replace( codedFilename.begin(), codedFilename.end(), '/', '_'); + std::replace( codedFilename.begin(), codedFilename.end(), '\\', '_'); + + + extractedFilePath = mExtractPath + "/" + codedFilename; + + f = fopen(extractedFilePath.c_str(), "rb"); + if (f != nullptr) { + + logger.debug("Using extracted image"); + + *size = 0; + while (true) { + size_t n = fread(mZipBuffer.data() + *size, 1, mZipBuffer.size(), f); + if (n == 0) break; + *size += n; + } + + fclose(f); + *data = (uint8_t *) mZipBuffer.data(); + return extractedFilePath; + } + f = fopen((mExtractPath + "/" + codedFilename).c_str(), "wb"); + if (f == nullptr) { + logger.error("Can't create file " + (mExtractPath + "/" + filename)); + extractedFilePath = ""; + } + } + *size = 0; zip_file_t *zipFile = zip_fopen(mZipArchive, filename.c_str(), 0); while (true) { size_t n = zip_fread(zipFile, mZipBuffer.data() + *size, mZipBuffer.size()); if (n == 0) break; + if (f != nullptr) { + fwrite(mZipBuffer.data() + *size, 1, n, f); + } *size += n; } zip_fclose(zipFile); + if (f != nullptr) { + fclose(f); + } *data = (uint8_t*)mZipBuffer.data(); + return extractedFilePath; } - inline void decompressFile(int id, uint8_t **data, size_t *size) { + inline std::string decompressFile(int id, uint8_t **data, size_t *size) { return decompressFile(mZipFilePath[id], data, size); } @@ -74,6 +119,7 @@ namespace CML { private: zip_t *mZipArchive; + std::string mZipPath, mExtractPath; List mZipFilePath; std::vector mZipBuffer; @@ -84,4 +130,4 @@ namespace CML { #endif -#endif \ No newline at end of file +#endif diff --git a/include/cml/capture/ZipStereopolisCapture.h b/include/cml/capture/ZipStereopolisCapture.h new file mode 100644 index 00000000..372005e2 --- /dev/null +++ b/include/cml/capture/ZipStereopolisCapture.h @@ -0,0 +1,115 @@ +#ifndef CML_ZIPSTEREOPOLISCAPTURE_H +#define CML_ZIPSTEREOPOLISCAPTURE_H + +#include "cml/config.h" + +#if CML_HAVE_LIBZIP + +#include "ZipCaptureHelper.h" + +namespace CML { + + class ZipStereopolisCapture : public AbstractMultithreadFiniteCapture, public ZipCaptureHelper { + + public: + inline ZipStereopolisCapture(const std::string &zipPath) { + + std::string extractPath = zipPath + "_uncompressed"; + if (!std::filesystem::is_directory(extractPath) || !std::filesystem::exists(extractPath)) { // Check if src folder exists + std::filesystem::create_directory(extractPath); + } + loadZip(zipPath, ".tif", extractPath); + + uint8_t *data; + size_t size; + decompressFile(1, &data, &size); + auto images = loadTiffImage(data, size); + mMask = loadPngImage(zipPath + ".mask.png").first.castToUChar(); + + mLookupTable = GrayLookupTable::exp(255, 1.005f); + + // int top = 1000, bottom = images.first.getHeight() - 1; + int top = 1000, bottom = 1600; + + for (int y = 0; y < images.first.getHeight(); y++) { + for (int x = 0; x < images.first.getWidth(); x++) { + if (mMask(x,y) < 128 /*|| images.first(x,y) > 254*/) { + if (y < images.first.getHeight() / 2) { + top = std::max(top, y); + } else { + bottom = std::min(bottom, y); + } + } + } + } + + + mCaptureImageGenerator = new CaptureImageGenerator(images.first.getWidth(), (bottom - top)); + + mCameraParameters = parseInternalStereopolisCalibration(zipPath + ".xml", mCaptureImageGenerator->getOutputSize(), top, bottom); + + + + } + + inline int remaining() final { + return imageNumbers() - mCurrentImage; + } + + inline int imageNumbers() final { + return getImageNumber(); + } + + protected: + inline Ptr multithreadNext() { + uint8_t *data; + size_t size; + std::string decompressedFilePath = decompressFile(mCurrentImage, &data, &size); + auto images = loadTiffImage(data, size); + /*if (mCurrentImage < 10) { + images.second.horizontalFlip().saveBmp(decompressedFilePath + ".bmp"); + }*/ + // images.first = images.second.toGrayImage(); + //images.first = images.first * (255.0f / mImageMax); + + for (int y = 0; y < images.first.getHeight(); y++) { + for (int x = 0; x < images.first.getWidth(); x++) { + if (mMask(x,y) < 128) { + images.first(x,y) = std::numeric_limits::quiet_NaN(); + images.second(x,y) = ColorRGBA(0,255,0,255); + } + /* else if (images.first(x,y) > 255.9) { + images.first(x,y) = std::numeric_limits::quiet_NaN(); + images.second(x,y) = ColorRGBA(0,0,0,0); + } */ + } + } + + CaptureImageMaker imageMaker = mCaptureImageGenerator->create(); + imageMaker.setImage(images.first) + .setImage(images.second) + .setPath(decompressedFilePath) + .setTime((scalar_t)mCurrentImage / 10.0) + .setCalibration(mCameraParameters) + .setLut(&mLookupTable); + + mCurrentImage++; + + return imageMaker.generate(); + } + + private: + CaptureImageGenerator *mCaptureImageGenerator; + InternalCalibration *mCameraParameters; + int mCurrentImage = 1; + GrayImage mMask; + GrayLookupTable mLookupTable; + + + }; + +} + +#endif + +#endif diff --git a/include/cml/capture/ZipTiffCapture.h b/include/cml/capture/ZipTiffCapture.h deleted file mode 100644 index 1a2efc9c..00000000 --- a/include/cml/capture/ZipTiffCapture.h +++ /dev/null @@ -1,160 +0,0 @@ -#ifndef CML_ZIPTIFFCAPTURE_H -#define CML_ZIPTIFFCAPTURE_H - -#include "cml/config.h" - -#if CML_HAVE_LIBZIP - -#include "ZipCaptureHelper.h" - - -namespace CML { - - class ZipTiffCapture : public AbstractMultithreadFiniteCapture, public ZipCaptureHelper { - - public: - inline ZipTiffCapture(const std::string &zipPath) { - logger.important("open zip file"); - loadZip(zipPath, ".tif"); - - uint8_t *data; - size_t size; - decompressFile(1, &data, &size); - logger.important("load tiff image"); - auto images = loadTiffImage(data, size); - logger.important("load mask"); - mMask = loadPngImage(zipPath + ".mask.png").first.castToUChar(); - logger.important("mask loaded"); - mMask.saveBmp("/home/tdaumain/mask.bmp"); - - //Add the real Time of the images - // Parse Time file - logger.important("Parsing time file..."); - std::ifstream timesFile; - timesFile.open((zipPath + ".times.txt").c_str()); - if (timesFile.is_open()) - { - while(!timesFile.eof() && timesFile.good()) - { - char buf[1000]; - timesFile.getline(buf, 1000); - int id; - double stamp; - if(2 == sscanf(buf, "%d %lf", &id, &stamp)) - { - mTimestamps.push_back(stamp); - } - - } - timesFile.close(); - } else { - throw std::runtime_error("Missing times.txt for STEREOPOLIS Dataset"); - } - - - int histogram[256]; - for (int i = 0; i < 256; i++) { - histogram[i] = 0; - } - for (int y = 0; y < images.first.getHeight(); y++) { - for (int x = 0; x < images.first.getWidth(); x++) { - histogram[(int)images.first(x,y)]+=1; - } - } - int threshold = (images.first.getWidth() * images.first.getHeight()) * 0.999; - for (int i = 1; i < 256; i++) { - histogram[i] += histogram[i - 1]; - if (histogram[i] > threshold) { - mImageMax = i; - break; - } - } - - mLookupTable = GrayLookupTable::exp(255, 1.005f); - - images.first = images.first * (255.0f / mImageMax); - - //crop with mask - int top = 0, bottom = images.first.getHeight() - 1; - - for (int y = 0; y < images.first.getHeight(); y++) { - for (int x = 0; x < images.first.getWidth(); x++) { - if (mMask(x,y) < 128 || images.first(x,y) > 255.9) { - if (y < images.first.getHeight() / 2) { - top = std::max(top, y); - } else { - bottom = std::min(bottom, y); - } - } - } - } - logger.important("TOP = " + std::to_string(top) + " BOTTOM = " + std::to_string(bottom)); - - - logger.important("create captureimagegenrator"); - mCaptureImageGenerator = new CaptureImageGenerator(images.first.getWidth(), (bottom - top)); - - logger.important("Internal Calibration"); - mCameraParameters = parseInternalStereopolisCalibration(zipPath + ".xml", mCaptureImageGenerator->getOutputSize(), top, bottom); - - - logger.important("ZipTiffCapture created"); - - } - - inline int remaining() final { - return imageNumbers() - mCurrentImage; - } - - inline int imageNumbers() final { - return getImageNumber(); - } - - protected: - inline Ptr multithreadNext() { - uint8_t *data; - size_t size; - decompressFile(mCurrentImage, &data, &size); - auto images = loadTiffImage(data, size); - - images.first = images.first * (255.0f / mImageMax); - - for (int y = 0; y < images.first.getHeight(); y++) { - for (int x = 0; x < images.first.getWidth(); x++) { - if (mMask(x,y) < 128) { - images.first(x,y) = std::numeric_limits::quiet_NaN(); - images.second(x,y) = ColorRGBA(0,0,0,0); - } - } - } - - CaptureImageMaker imageMaker = mCaptureImageGenerator->create(); - imageMaker.setImage(images.first) - .setImage(images.second) - .setPath(getFilename(mCurrentImage)) - /*.setTime((scalar_t)mCurrentImage / 10.0)*/.setTime(mTimestamps[mCurrentImage]) - .setCalibration(mCameraParameters) - .setLut(&mLookupTable); - - mCurrentImage++; - - return imageMaker.generate(); - } - - private: - CaptureImageGenerator *mCaptureImageGenerator; - InternalCalibration *mCameraParameters; - int mCurrentImage = 1; - GrayImage mMask; - GrayLookupTable mLookupTable; - float mImageMax = 0; - std::vector mTimestamps; - - - }; - -} - -#endif - -#endif diff --git a/src/cml/base/AbstractSlam.cpp b/src/cml/base/AbstractSlam.cpp index a9badac1..9286e98a 100644 --- a/src/cml/base/AbstractSlam.cpp +++ b/src/cml/base/AbstractSlam.cpp @@ -1,6 +1,19 @@ #include +#include +#include + +std::string to_string(const CML::Camera& cam){ + CML::Matrix33 m = cam.getRotationMatrix(); + return "Translation:\n" + + std::to_string(cam.getTranslation()(0)) + " " + std::to_string(cam.getTranslation()(1)) + " " + std::to_string(cam.getTranslation()(2)) + + "\nRotation :\n" + + std::to_string(m(0,0)) + " " + std::to_string(m(0,1)) + " " + std::to_string(m(0,2)) + "\n" + + std::to_string(m(1,0)) + " " + std::to_string(m(1,1)) + " " + std::to_string(m(1,2)) + "\n" + + std::to_string(m(2,0)) + " " + std::to_string(m(2,1)) + " " + std::to_string(m(2,2)); +} + CML::AbstractSlam::AbstractSlam() : AbstractFunction(nullptr), mMap(), mGarbageCollectorInstance(mMap.getGarbageCollector().newInstance()) { //mUnusedParameters.set_empty_key("reserved.empty"); //mUnusedParameters.set_deleted_key("reserved.deleted"); @@ -10,6 +23,56 @@ CML::AbstractSlam::AbstractSlam() : AbstractFunction(nullptr), mMap(), mGarbageC mIsStopped = true; } +// Add groundtruth positions of the camera in slam +void CML::AbstractSlam::addGroundtruth(std::string pathGroundtruth){ + std::ifstream groundtruth; + groundtruth.open(pathGroundtruth.c_str()); + if (groundtruth.is_open()) + { + groundtruth.ignore(80,'\n'); + while(!groundtruth.eof() && groundtruth.good()) + { + char buf[1000]; + groundtruth.getline(buf, 1000); + int id; + double time, r11, r12, r13, tx, r21, r22, r23, ty, r31, r32, r33, tz; + if(13 == sscanf(buf, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf ", &time, &tx, &ty, &tz, &r11, &r12, &r13, &r21, &r22, &r23, &r31, &r32, &r33)) + { + //camera rotation + Matrix33 rot; + rot << r11, r12, r13, + r21, r22, r23, + r31, r32, r33; + //camera translation + Vector3 translation(tx, ty, tz); + //save camera position + Camera cam(translation, rot); + mGroundtruths.push_back(cam); + //save time + mTimes.push_back(time); + } + + } + groundtruth.close(); + } else { + throw std::runtime_error("groundtruth path not found"); + } + + mHaveGroundtruth = true; + + // add correction in translation and rotation + // correct_translation = getGroundtruth(99).getTranslation(); + // Matrix33 Rx { + // {1, 0, 0}, + // {0, std::cos(M_PI/2), -std::sin(M_PI/2)}, + // {0, std::sin(M_PI/2), std::cos(M_PI/2)} + // }; + // correct_rotation = Rx; + // correct_cam = getGroundtruth(104).to(Camera::identity()); + correct_cam = getGroundtruth(104); + logger.important(to_string(correct_cam)); +} + void CML::AbstractSlam::start(Ptr capture) { interrupt(); @@ -175,10 +238,51 @@ CML::Ptr CML::AbstractSlam::getNextFrame() { return currentFrame; } -void CML::AbstractSlam::addFrame(PFrame currentFrame) { + +std::string ss; +int i = 0; +void CML::AbstractSlam::addFrame(PFrame currentFrame){ + i++; if (mMap.getFramesNumber() > 0) { - currentFrame->setCamera(mMap.getLastFrame()->getCamera()); - currentFrame->setExposureParameters(mMap.getLastFrame()->getExposure()); + //if slam have gps camera positions, initialize the new frame with gps data + if(mHaveGroundtruth && getCapture()->imageNumbers() > 115){ + scalar_t timeFrame = getCapture()->getTime(); // time of the current frame + int index = getCapture()->imageNumbers(); + logger.important(" index : " + std::to_string(index)); + if(timeFrame != mTimes[index]){ + logger.error("Time not synchronized with gps"); + throw std::runtime_error("error in gps data. Time not syncronized\n"); + }else{ + //initialize cam frame position + Camera cam = getGroundtruth(index); + + Camera newCam = cam.compose(correct_cam.inverse()); + // Camera newCam = correct_cam.to(cam); + Vector3 t = newCam.getTranslation(); + //scale ?? + + Camera c(t, newCam.getRotationMatrix()); + Vector3 vec = c.getTranslation(); + // ss = ss + std::to_string(vec(0)) + " " + std::to_string(vec(1)) + " " + std::to_string(vec(2)) + "\n"; + // if(i == 150) { + // logger.important(ss); + // } + // logger.important(to_string(newCam)); + // currentFrame->setCamera(newCam); + // logger.important(to_string(c)); + currentFrame->setCamera(c); + // currentFrame->setCamera(newCam); + } + }else{ + Vector3 t = mMap.getLastFrame()->getCamera().getTranslation(); + ss = ss + std::to_string(t(0)) + " " + std::to_string(t(1)) + " " + std::to_string(t(2)) + "\n"; + if(i == 150) { + logger.important(ss); + } + logger.important(to_string(mMap.getLastFrame()->getCamera())); + currentFrame->setCamera(mMap.getLastFrame()->getCamera()); + } + currentFrame->setExposureParameters(mMap.getLastFrame()->getExposure()); } mMap.addFrame(currentFrame); @@ -198,4 +302,3 @@ void CML::AbstractSlam::addFrame(PFrame currentFrame) { CML::Ptr CML::AbstractSlam::getLastCaptureFrame() { return mLastCaptureImage; } - diff --git a/src/cml/slam/modslam.cpp b/src/cml/slam/modslam.cpp index a6951804..0186e867 100644 --- a/src/cml/slam/modslam.cpp +++ b/src/cml/slam/modslam.cpp @@ -20,7 +20,7 @@ #if CML_HAVE_LIBZIP #include -#include +#include #endif #include @@ -82,7 +82,7 @@ Ptr loadDataset(const std::string &path, const bool& } try { - Ptr capture = new CML::ZipTiffCapture(path); + Ptr capture = new CML::ZipStereopolisCapture(path); return capture; } catch (const std::exception &e) { logger.error(e.what()); @@ -315,8 +315,18 @@ int main(int argc, char *argv[]) program.add_argument("-s", "--save").nargs(1).help("Save the images").action([&saveImagePath](const std::string &value) { saveImagePath = value; }); + program.add_argument("-v", "--verbose").nargs(0).help("Verbose").default_value(false).implicit_value(true); + + program.add_argument("-a", "--groundtruth").nargs(1).help("add groundtruth path").action([&slam](const std::string &value){ + slam->addGroundtruth(value); + }); + program.parse_args(argc, argv); + if (program["--verbose"] == true) { + logger.setLogLevel(CML::MORE); + } + #if CML_ENABLE_GUI if (program["--gui"] == true) { executionMode = GUI; diff --git a/src/cml/slam/modslam/Hybrid.cpp b/src/cml/slam/modslam/Hybrid.cpp index b29a3302..685e9585 100644 --- a/src/cml/slam/modslam/Hybrid.cpp +++ b/src/cml/slam/modslam/Hybrid.cpp @@ -370,6 +370,7 @@ void Hybrid::trackWithOrbAndDsoRefinement(PFrame currentFrame) { void Hybrid::trackWithDso(PFrame currentFrame) { assertThrow(mEnableDirect.b(), "You can't call this function with direct disabled"); + logger.debug("track with dso"); currentFrame->setGroup(DSOTRACKEDFRAME, true); mTrackedWithIndirect = false; From 742fd36c346b91279204ff8473b41d1fc72a17c7 Mon Sep 17 00:00:00 2001 From: ThomasD Date: Tue, 29 Mar 2022 14:58:04 +0200 Subject: [PATCH 4/4] gitignore update --- .gitignore | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 6bb9ca19..06693a6a 100644 --- a/.gitignore +++ b/.gitignore @@ -34,8 +34,8 @@ *.app # Build folder -#*/build -# */build-* +*/build +*/build-* */.idea evaluation/results evaluation/resources