diff --git a/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt b/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt index a1cef9a5e..e9bef643b 100644 --- a/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt +++ b/InfiniTAM/Apps/InfiniTAM/CMakeLists.txt @@ -15,6 +15,7 @@ SET(targetname InfiniTAM) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseCUDA.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseFFmpeg.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseGLUT.cmake) +INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseLibRoyale.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenGL.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenMP.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenNI.cmake) @@ -54,6 +55,7 @@ INCLUDE(${PROJECT_SOURCE_DIR}/cmake/SetCUDAAppTarget.cmake) TARGET_LINK_LIBRARIES(${targetname} InputSource ITMLib MiniSlamGraphLib ORUtils FernRelocLib) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkFFmpeg.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkGLUT.cmake) +INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkLibRoyale.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkOpenGL.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkOpenNI.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkPNG.cmake) diff --git a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp index ab3dc9d0f..3269ff8f8 100644 --- a/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp +++ b/InfiniTAM/Apps/InfiniTAM/InfiniTAM.cpp @@ -8,6 +8,7 @@ #include "../../InputSource/OpenNIEngine.h" #include "../../InputSource/Kinect2Engine.h" #include "../../InputSource/LibUVCEngine.h" +#include "../../InputSource/PicoFlexxEngine.h" #include "../../InputSource/RealSenseEngine.h" #include "../../InputSource/LibUVCEngine.h" #include "../../InputSource/RealSenseEngine.h" @@ -124,6 +125,17 @@ static void CreateDefaultImageSource(ImageSourceEngine* & imageSource, IMUSource imageSource = NULL; } } + + if (imageSource == NULL) + { + printf("trying PMD PicoFlexx device\n"); + imageSource = new PicoFlexxEngine(calibFile); + if (imageSource->getDepthImageSize().x == 0) + { + delete imageSource; + imageSource = NULL; + } + } } int main(int argc, char** argv) diff --git a/InfiniTAM/Apps/InfiniTAM/UIEngine.cpp b/InfiniTAM/Apps/InfiniTAM/UIEngine.cpp index a1935779d..4e867b5c4 100644 --- a/InfiniTAM/Apps/InfiniTAM/UIEngine.cpp +++ b/InfiniTAM/Apps/InfiniTAM/UIEngine.cpp @@ -272,6 +272,7 @@ void UIEngine::glutKeyUpFunction(unsigned char key, int x, int y) ITMBasicEngine *basicEngine = dynamic_cast*>(uiEngine->mainEngine); if (basicEngine != NULL) basicEngine->resetAll(); } + break; case 'k': { printf("saving scene to disk ... "); diff --git a/InfiniTAM/ITMLib/Utils/ITMLibSettings.cpp b/InfiniTAM/ITMLib/Utils/ITMLibSettings.cpp index ceea0bdf5..fa078470b 100644 --- a/InfiniTAM/ITMLib/Utils/ITMLibSettings.cpp +++ b/InfiniTAM/ITMLib/Utils/ITMLibSettings.cpp @@ -39,7 +39,7 @@ ITMLibSettings::ITMLibSettings(void) useBilateralFilter = false; /// what to do on tracker failure: ignore, relocalise or stop integration - not supported in loop closure version - behaviourOnFailure = FAILUREMODE_IGNORE; + behaviourOnFailure = FAILUREMODE_RELOCALISE; /// switch between various library modes - basic, with loop closure, etc. libMode = LIBMODE_BASIC; diff --git a/InfiniTAM/InputSource/CMakeLists.txt b/InfiniTAM/InputSource/CMakeLists.txt index 5e64db218..0969aaa36 100644 --- a/InfiniTAM/InputSource/CMakeLists.txt +++ b/InfiniTAM/InputSource/CMakeLists.txt @@ -22,6 +22,7 @@ INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseCUDA.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseFFmpeg.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseGLUT.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseKinect2.cmake) +INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseLibRoyale.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenGL.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenMP.cmake) INCLUDE(${PROJECT_SOURCE_DIR}/cmake/UseOpenNI.cmake) @@ -41,6 +42,7 @@ IMUSourceEngine.cpp Kinect2Engine.cpp LibUVCEngine.cpp OpenNIEngine.cpp +PicoFlexxEngine.cpp RealSenseEngine.cpp ) @@ -53,6 +55,7 @@ IMUSourceEngine.h Kinect2Engine.h LibUVCEngine.h OpenNIEngine.h +PicoFlexxEngine.h RealSenseEngine.h ) diff --git a/InfiniTAM/InputSource/PicoFlexxEngine.cpp b/InfiniTAM/InputSource/PicoFlexxEngine.cpp new file mode 100644 index 000000000..eb32d3d99 --- /dev/null +++ b/InfiniTAM/InputSource/PicoFlexxEngine.cpp @@ -0,0 +1,225 @@ +// Copyright 2017 Akos Maroy and the authors of InfiniTAM + +#include "PicoFlexxEngine.h" + +#ifdef COMPILE_WITH_LibRoyale + +#include +#include +#include +#include + +#ifdef WIN32 +#include +#define ROYALE_SLEEP_MS(x) Sleep(x) +#else +#include +#define ROYALE_SLEEP_MS(x) usleep(x * 1000) +#endif + +#include + +using namespace InputSource; +using namespace royale; +using namespace std; + +class PicoFlexxEngine::PrivateData : public IDepthDataListener +{ +public: + PicoFlexxEngine *engine; + std::unique_ptr cameraDevice; + vector rgbImage; + vector depthImage; + mutex mtx; + + explicit PrivateData(PicoFlexxEngine *pfe) : engine(pfe), depthImage(0) {} + ~PrivateData() {} + + void onNewData(const DepthData *data); +}; + + +void PicoFlexxEngine::PrivateData::onNewData(const DepthData *data) +{ + lock_guard lock(mtx); + + // handle the grayscale image + engine->imageSize_rgb = Vector2i(data->width, data->height); + + rgbImage.clear(); + rgbImage.reserve(data->points.size()); + + // copy grayscale image data into an RGB data set + for (int pointId = 0; pointId < data->points.count(); pointId++) + { + // PicoFlexx seems to return 0 when bright, and up to FF0 when totally dark + // convert this into a value into an 8 bit value + unsigned char charValue = data->points[pointId].grayValue >> 4; + Vector4u pixel; + pixel.x = pixel.y = pixel.z = pixel.w = charValue; + this->rgbImage.push_back(pixel); + }; + + // handle the depth image + engine->imageSize_d = Vector2i(data->width, data->height); + + depthImage.clear(); + depthImage.reserve(data->points.size()); + + // copy depth image data, converting meters in float to millimeters in short + for (int pointId = 0; pointId < data->points.count(); pointId++) + { + // do not copy if confidence is low. confidence is 0 when bad, 255 when good + // it seems there are no intermediate values, still let's cut at 128 + const DepthPoint &dd = data->points[pointId]; + this->depthImage.push_back(dd.depthConfidence > 128 ? (short)(dd.z * 1000.0) : 0); + }; +} + +PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceURI, const bool useInternalCalibration, + Vector2i requested_imageSize_rgb, Vector2i requested_imageSize_d) + : BaseImageSourceEngine(calibFilename) +{ + this->imageSize_d = Vector2i(0, 0); + this->imageSize_rgb = Vector2i(0, 0); + + data = new PrivateData(this); + + // the camera manager will query for a connected camera + { + CameraManager manager; + + royale::Vector camlist = manager.getConnectedCameraList(); + cout << "Detected " << camlist.size() << " camera(s)." << endl; + if (!camlist.empty()) + { + cout << "CamID for first device: " << camlist.at(0).c_str() << " with a length of (" << camlist.at(0).length() << ")" << endl; + data->cameraDevice = manager.createCamera(camlist[0]); + } + } + // the camera device is now available and CameraManager can be deallocated here + + if (data->cameraDevice == nullptr) + { + cerr << "Cannot create the camera device" << endl; + return; + } + + // IMPORTANT: call the initialize method before working with the camera device + if (data->cameraDevice->initialize() != CameraStatus::SUCCESS) + { + cerr << "Cannot initialize the camera device" << endl; + return; + } + + // set camera parameters + LensParameters lensParams; + if (data->cameraDevice->getLensParameters(lensParams) != CameraStatus::SUCCESS) { + cerr << "Cannot determine lens parameters" << endl; + return; + } + + this->calib.intrinsics_d.SetFrom(lensParams.focalLength.first, lensParams.focalLength.second, + lensParams.principalPoint.first, lensParams.principalPoint.second); + this->calib.intrinsics_rgb.SetFrom(lensParams.focalLength.first, lensParams.focalLength.second, + lensParams.principalPoint.first, lensParams.principalPoint.second); + + royale::Vector useCases; + royale::CameraStatus status = data->cameraDevice->getUseCases(useCases); + + if (status != CameraStatus::SUCCESS || useCases.empty()) + { + cerr << "No use cases are available" << endl; + cerr << "getUseCases() returned: " << getErrorString(status) << endl; + return; + } + + // list the available use cases + cout << "Available Pico Flexx use cases:" << endl; + for (size_t caseId = 0; caseId < useCases.size(); caseId++) + { + cout << useCases[caseId] << endl; + } + + // register a data listener + if (data->cameraDevice->registerDataListener(data) != CameraStatus::SUCCESS) + { + cerr << "Error registering data listener" << endl; + return; + } + + // set an operation mode + // TODO allow setting this from the command line + if (data->cameraDevice->setUseCase("MODE_9_10FPS_1000") != CameraStatus::SUCCESS) + { + cerr << "Error setting use case" << endl; + return; + } + + // start capture mode + if (data->cameraDevice->startCapture() != CameraStatus::SUCCESS) + { + cerr << "Error starting the capturing" << endl; + return; + } + + // waiting for the capture to start and 'data' to be populated + ROYALE_SLEEP_MS(1000); +} + +PicoFlexxEngine::~PicoFlexxEngine() +{ + if (data) + { + // stop capture mode + if (data->cameraDevice) + if (data->cameraDevice->stopCapture() != CameraStatus::SUCCESS) cerr << "Error stopping the capturing" << endl; + + delete data; + } +} + +void PicoFlexxEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage) +{ + lock_guard lock(data->mtx); + + // copy the color info +#ifdef PROVIDE_RGB + Vector4u *rgb = rgbImage->GetData(MEMORYDEVICE_CPU); + if (data->rgbImage.size()) { + memcpy(rgb, data->rgbImage.data(), rgbImage->dataSize * sizeof(Vector4u)); + } + else + memset(rgb, 0, rgbImage->dataSize * sizeof(Vector4u)); +#else + imageSize_rgb = Vector2i(0, 0); +#endif + + // copy the depth info + short *depth = rawDepthImage->GetData(MEMORYDEVICE_CPU); + if (data->depthImage.size()) { + memcpy(depth, data->depthImage.data(), rawDepthImage->dataSize * sizeof(short)); + } + else memset(depth, 0, rawDepthImage->dataSize * sizeof(short)); +} + +bool PicoFlexxEngine::hasMoreImages(void) const { return data != NULL; } +Vector2i PicoFlexxEngine::getDepthImageSize(void) const { return data != NULL ? imageSize_d : Vector2i(0, 0); } +Vector2i PicoFlexxEngine::getRGBImageSize(void) const { return data != NULL ? imageSize_rgb : Vector2i(0, 0); } + +#else + +using namespace InputSource; + +PicoFlexxEngine::PicoFlexxEngine(const char *calibFilename, const char *deviceURI, const bool useInternalCalibration, Vector2i requested_imageSize_rgb, Vector2i requested_imageSize_d) + : BaseImageSourceEngine(calibFilename) +{ + printf("compiled without LibRoyale support\n"); +} +PicoFlexxEngine::~PicoFlexxEngine() {} +void PicoFlexxEngine::getImages(ITMUChar4Image *rgbImage, ITMShortImage *rawDepthImage) { } +bool PicoFlexxEngine::hasMoreImages(void) const { return false; } +Vector2i PicoFlexxEngine::getDepthImageSize(void) const { return Vector2i(0, 0); } +Vector2i PicoFlexxEngine::getRGBImageSize(void) const { return Vector2i(0, 0); } + +#endif \ No newline at end of file diff --git a/InfiniTAM/InputSource/PicoFlexxEngine.h b/InfiniTAM/InputSource/PicoFlexxEngine.h new file mode 100644 index 000000000..4d8a1ee5e --- /dev/null +++ b/InfiniTAM/InputSource/PicoFlexxEngine.h @@ -0,0 +1,27 @@ +// Copyright 2017 Akos Maroy + +#pragma once + +#include "ImageSourceEngine.h" + +namespace InputSource +{ + class PicoFlexxEngine : public BaseImageSourceEngine + { + private: + class PrivateData; + PrivateData *data; + Vector2i imageSize_rgb, imageSize_d; + bool colorAvailable, depthAvailable; + + public: + explicit PicoFlexxEngine(const char *calibFilename, const char *deviceURI = NULL, const bool useInternalCalibration = false, + Vector2i imageSize_rgb = Vector2i(224, 171), Vector2i imageSize_d = Vector2i(224, 171)); + ~PicoFlexxEngine(); + + bool hasMoreImages(void) const; + void getImages(ITMUChar4Image *rgb, ITMShortImage *rawDepth); + Vector2i getDepthImageSize(void) const; + Vector2i getRGBImageSize(void) const; + }; +} \ No newline at end of file diff --git a/InfiniTAM/cmake/FindLibRoyale.cmake b/InfiniTAM/cmake/FindLibRoyale.cmake new file mode 100644 index 000000000..ad1461133 --- /dev/null +++ b/InfiniTAM/cmake/FindLibRoyale.cmake @@ -0,0 +1,17 @@ +find_path(LibRoyale_ROOT royale_license.txt + PATHS ${LibRoyale_ROOT} "C:/Program Files/royale/2.3.0.92" "/usr/local") + +find_library(LibRoyale_LIBRARY + NAMES royale + PATHS "${LibRoyale_ROOT}/lib" {CMAKE_LIB_PATH} +) + +find_path(LibRoyale_INCLUDE_DIR royale.hpp + PATHS "${LibRoyale_ROOT}/include" +) + +if (LibRoyale_LIBRARY AND LibRoyale_INCLUDE_DIR AND LibRoyale_ROOT) + set(LibRoyale_FOUND TRUE) +else () + set(LibRoyale_FOUND FALSE) +endif() \ No newline at end of file diff --git a/InfiniTAM/cmake/LinkLibRoyale.cmake b/InfiniTAM/cmake/LinkLibRoyale.cmake new file mode 100644 index 000000000..79eee756f --- /dev/null +++ b/InfiniTAM/cmake/LinkLibRoyale.cmake @@ -0,0 +1,11 @@ +####################### +# LinkLibRoyale.cmake # +####################### + +IF(WITH_LIBROYALE) + TARGET_LINK_LIBRARIES(${targetname} ${LibRoyale_LIBRARY}) + + IF(MSVC_IDE) + ADD_CUSTOM_COMMAND(TARGET ${targetname} POST_BUILD COMMAND ${CMAKE_COMMAND} -E copy_if_different "${LibRoyale_ROOT}/bin/royale.dll" "$") + ENDIF() +ENDIF() \ No newline at end of file diff --git a/InfiniTAM/cmake/UseLibRoyale.cmake b/InfiniTAM/cmake/UseLibRoyale.cmake new file mode 100644 index 000000000..1a0a30ed6 --- /dev/null +++ b/InfiniTAM/cmake/UseLibRoyale.cmake @@ -0,0 +1,11 @@ +###################### +# UseLibRoyale.cmake # +###################### + +OPTION(WITH_LIBROYALE "Build with LibRoyale support?" OFF) + +IF(WITH_LIBROYALE) + FIND_PACKAGE(LibRoyale REQUIRED) + INCLUDE_DIRECTORIES(${LibRoyale_INCLUDE_DIR}) + ADD_DEFINITIONS(-DCOMPILE_WITH_LibRoyale) +ENDIF()