diff --git a/CMakeLists.txt b/CMakeLists.txt index 9ccd1d59..3765fb94 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 11) -SET(RTABMAP_PATCH_VERSION 6) +SET(RTABMAP_PATCH_VERSION 7) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) @@ -136,6 +136,7 @@ option(WITH_TORO "Include TORO support" ON) option(WITH_VERTIGO "Include Vertigo support" ON) option(WITH_CVSBA "Include cvsba support" ON) option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) +option(WITH_ZED "Include ZED sdk support" ON) FIND_PACKAGE(OpenCV REQUIRED QUIET) FIND_PACKAGE(PCL 1.7 REQUIRED QUIET) @@ -259,12 +260,41 @@ IF(WITH_FLYCAPTURE2) ENDIF(WITH_FLYCAPTURE2) IF(WITH_CVSBA) - FIND_PACKAGE(cvsba QUIET) + FIND_PACKAGE(cvsba QUIET) IF(cvsba_FOUND) MESSAGE(STATUS "Found cvsba: ${cvsba_INCLUDE_DIRS}") ENDIF(cvsba_FOUND) ENDIF(WITH_CVSBA) +IF(WITH_ZED) + IF(WIN32) # Windows + SET(ZED_INCLUDE_DIRS $ENV{ZED_INCLUDE_DIRS}) + if (CMAKE_CL_64) # 64 bits + SET(ZED_LIBRARIES $ENV{ZED_LIBRARIES_64}) + else(CMAKE_CL_64) # 32 bits + message("32bits compilation is no more available with CUDA7.0") + endif(CMAKE_CL_64) + SET(ZED_LIBRARY_DIR $ENV{ZED_LIBRARY_DIR}) + IF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS) + SET(ZED_FOUND TRUE) + LINK_DIRECTORIES( ${LINK_DIRECTORIES} ${ZED_LIBRARY_DIR}) + ENDIF(ZED_LIBRARIES AND ZED_INCLUDE_DIRS) + ELSE() # Linux + find_package(ZED 0.9) + ENDIF(WIN32) + + IF(ZED_FOUND) + MESSAGE(STATUS "Found ZED sdk: ${ZED_INCLUDE_DIRS}") + ## look for CUDA + find_package(CUDA) + IF(CUDA_FOUND) + MESSAGE(STATUS "Found CUDA: ${CUDA_INCLUDE_DIRS}") + ELSE() + MESSAGE(FATAL_ERROR "CUDA is required to build with Zed sdk! Set -DWITH_ZED=OFF if you don't have CUDA.") + ENDIF() + ENDIF(ZED_FOUND) +ENDIF(WITH_ZED) + ####### OSX BUNDLE CMAKE_INSTALL_PREFIX ####### IF(APPLE AND BUILD_AS_BUNDLE) IF(Qt5_FOUND OR (QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)) @@ -327,6 +357,9 @@ ENDIF(NOT DC1394_FOUND) IF(NOT FlyCapture2_FOUND) SET(FLYCAPTURE2 "//") ENDIF(NOT FlyCapture2_FOUND) +IF(NOT ZED_FOUND) + SET(ZED "//") +ENDIF(NOT ZED_FOUND) IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3)) SET(OPENCV3 "//") ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3)) @@ -601,6 +634,18 @@ ELSE() MESSAGE(STATUS " With cvsba = NO (cvsba not found)") ENDIF() +IF(ZED_FOUND) +IF(CUDA_FOUND) +MESSAGE(STATUS " With ZED = YES (With CUDA)") +ELSE() +MESSAGE(STATUS " With ZED = YES (Without CUDA)") +ENDIF() +ELSEIF(NOT WITH_ZED) +MESSAGE(STATUS " With ZED = NO (WITH_ZED=OFF)") +ELSE() +MESSAGE(STATUS " With ZED = NO (ZED sdk not found)") +ENDIF() + IF(QT4_FOUND) MESSAGE(STATUS " With Qt4 = YES (License: Open Source or Commercial)") ELSEIF(Qt5_FOUND) diff --git a/Version.h.in b/Version.h.in index 9124fa20..bf441bf4 100644 --- a/Version.h.in +++ b/Version.h.in @@ -49,6 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @CVSBA@#define RTABMAP_CVSBA @DC1394@#define RTABMAP_DC1394 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2 +@ZED@#define RTABMAP_ZED #endif /* VERSION_H_ */ diff --git a/corelib/include/rtabmap/core/CameraStereo.h b/corelib/include/rtabmap/core/CameraStereo.h index 0d76917e..eee499b7 100644 --- a/corelib/include/rtabmap/core/CameraStereo.h +++ b/corelib/include/rtabmap/core/CameraStereo.h @@ -39,6 +39,14 @@ namespace FlyCapture2 class Camera; } +namespace sl +{ +namespace zed +{ +class Camera; +} +} + namespace rtabmap { @@ -94,6 +102,32 @@ private: void * triclopsCtx_; // TriclopsContext }; +///////////////////////// +// CameraStereoZED +///////////////////////// +class RTABMAP_EXP CameraStereoZed : + public Camera +{ +public: + static bool available(); + +public: + CameraStereoZed(bool rgbdMode, float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); + virtual ~CameraStereoZed(); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); + virtual bool isCalibrated() const; + virtual std::string getSerial() const; + +protected: + virtual SensorData captureImage(); + +private: + sl::zed::Camera * zed_; + StereoCameraModel stereoModel_; + bool rgbdMode_; +}; + ///////////////////////// // CameraStereoImages ///////////////////////// @@ -147,6 +181,10 @@ public: bool rectifyImages = false, float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity()); + CameraStereoVideo( + int device, + float imageRate = 0.0f, + const Transform & localTransform = Transform::getIdentity()); virtual ~CameraStereoVideo(); virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); @@ -162,6 +200,8 @@ private: bool rectifyImages_; StereoCameraModel stereoModel_; std::string cameraName_; + CameraVideo::Source src_; + int usbDevice_; }; } // namespace rtabmap diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index b55db570..bc310c03 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -221,9 +221,9 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)."); RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)"); #ifdef RTABMAP_NONFREE - RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK."); + RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); #else - RTABMAP_PARAM(Kp, DetectorStrategy, int, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK."); + RTABMAP_PARAM(Kp, DetectorStrategy, int, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); #endif RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, true, "Use of the td-idf strategy to compute the likelihood."); RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized."); @@ -396,7 +396,17 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation."); RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation."); RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform."); - RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK."); +#ifndef RTABMAP_NONFREE +#ifdef RTABMAP_OPENCV3 + // OpenCV 3 without xFeatures2D module doesn't have BRIEF + RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); +#else + RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); +#endif +#else + RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); +#endif + RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); RTABMAP_PARAM(Vis, MaxDepth, float, 0.0, "Max depth of the features (0 means no limit)."); RTABMAP_PARAM(Vis, MinDepth, float, 0.0, "Min depth of the features (0 means no limit)."); diff --git a/corelib/include/rtabmap/core/util2d.h b/corelib/include/rtabmap/core/util2d.h index b01e7f83..03c616e2 100644 --- a/corelib/include/rtabmap/core/util2d.h +++ b/corelib/include/rtabmap/core/util2d.h @@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include namespace rtabmap { @@ -68,7 +69,7 @@ void RTABMAP_EXP calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputA cv::Mat RTABMAP_EXP disparityFromStereoImages( const cv::Mat & leftImage, const cv::Mat & rightImage, - int type = CV_32FC1); // CV_32FC1 or CV_16SC1 + const ParametersMap & parameters = ParametersMap()); cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity, float fx, float baseline, diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 4923bb30..4536ca85 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -116,14 +117,16 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromStereoImages( int decimation = 1, float maxDepth = 0.0f, float minDepth = 0.0f, - std::vector * validIndices = 0); + std::vector * validIndices = 0, + const ParametersMap & parameters = ParametersMap()); pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( const SensorData & sensorData, int decimation = 1, float maxDepth = 0.0f, float minDepth = 0.0f, - std::vector * validIndices = 0); + std::vector * validIndices = 0, + const ParametersMap & parameters = ParametersMap()); /** * Create an RGB cloud from the images contained in SensorData. If there is only one camera, @@ -143,7 +146,8 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( int decimation = 1, float maxDepth = 0.0f, float minDepth = 0.0f, - std::vector * validIndices = 0); + std::vector * validIndices = 0, + const ParametersMap & parameters = ParametersMap()); pcl::PointCloud RTABMAP_EXP laserScanFromDepthImage( const cv::Mat & depthImage, diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index a1d5e186..ac77ca9a 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -209,6 +209,27 @@ IF(cvsba_FOUND) ) ENDIF(cvsba_FOUND) +IF(ZED_FOUND) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${ZED_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${ZED_LIBRARIES} + ) + IF(CUDA_FOUND) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${CUDA_LIBRARIES} + ) + ENDIF(CUDA_FOUND) +ENDIF(ZED_FOUND) + #################################### # Generate resources files #################################### diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp index 0b1adfa7..8c0a547a 100644 --- a/corelib/src/CameraRGB.cpp +++ b/corelib/src/CameraRGB.cpp @@ -722,7 +722,7 @@ CameraVideo::~CameraVideo() bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName) { - _guid.clear(); + _guid = cameraName; if(_capture.isOpened()) { _capture.release(); @@ -750,19 +750,22 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string } else { - unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID); - if(guid != 0 && guid != 0xffffffff) + if (_guid.empty()) { - _guid = uFormat("%08x", guid); + unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID); + if (guid != 0 && guid != 0xffffffff) + { + _guid = uFormat("%08x", guid); + } } // look for calibration files - if(!calibrationFolder.empty() && (!_guid.empty() || !cameraName.empty())) + if(!calibrationFolder.empty() && !_guid.empty()) { - if(!_model.load(calibrationFolder, (cameraName.empty()?_guid:cameraName))) + if(!_model.load(calibrationFolder, _guid)) { UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", - cameraName.empty()?_guid.c_str():cameraName.c_str(), calibrationFolder.c_str()); + _guid.c_str(), calibrationFolder.c_str()); } else { diff --git a/corelib/src/CameraStereo.cpp b/corelib/src/CameraStereo.cpp index ca1b114b..8ec0e4ed 100644 --- a/corelib/src/CameraStereo.cpp +++ b/corelib/src/CameraStereo.cpp @@ -48,6 +48,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #endif +#ifdef RTABMAP_ZED +#include +#endif + namespace rtabmap { @@ -731,6 +735,152 @@ SensorData CameraStereoFlyCapture2::captureImage() return data; } +// +// CameraStereoZED +// +bool CameraStereoZed::available() +{ +#ifdef RTABMAP_ZED + return true; +#else + return false; +#endif +} + +CameraStereoZed::CameraStereoZed(bool rgbdMode, float imageRate, const Transform & localTransform) : + Camera(imageRate, localTransform), + zed_(0), + rgbdMode_(rgbdMode) +{ +} + +CameraStereoZed::~CameraStereoZed() +{ +#ifdef RTABMAP_ZED + if(zed_) + { + delete zed_; + } +#endif +} + +bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName) +{ +#ifdef RTABMAP_ZED + if(zed_) + { + delete zed_; + zed_ = 0; + } + + if(zed_->isZEDconnected()) + { + zed_ = new sl::zed::Camera(sl::zed::HD720); // Use in Live Mode + //zed_ = new sl::zed::Camera(argv[1]); // Use in SVO playback mode + + int width = zed_->getImageSize().width; + int height = zed_->getImageSize().height; + + //init WITH self-calibration (- last parameter to false -) + sl::zed::ERRCODE err = zed_->init(sl::zed::MODE::PERFORMANCE, 0, true, false, false); + + // Quit if an error occurred + if (err != sl::zed::SUCCESS) + { + UERROR("ZED camera initialization failed: %s", sl::zed::errcode2str(err)); + delete zed_; + zed_ = 0; + return false; + } + } + else + { + UERROR("ZED camera initialization failed: ZED is not connected!"); + return false; + } + + sl::zed::StereoParameters * stereoParams = zed_->getParameters(); + sl::zed::resolution res = zed_->getImageSize(); + + stereoModel_ = StereoCameraModel( + stereoParams->LeftCam.fx, + stereoParams->LeftCam.fy, + stereoParams->LeftCam.cx, + stereoParams->LeftCam.cy, + stereoParams->baseline/1000.0f, + this->getLocalTransform(), + cv::Size(res.width, res.height)); + + return true; +#else + UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!"); +#endif + return false; +} + +bool CameraStereoZed::isCalibrated() const +{ + return stereoModel_.isValidForProjection(); +} + +std::string CameraStereoZed::getSerial() const +{ +#ifdef RTABMAP_ZED + if(zed_) + { + return uFormat("%x", zed_->getZEDSerial()); + } +#endif + return ""; +} + +SensorData CameraStereoZed::captureImage() +{ + SensorData data; +#ifdef RTABMAP_ZED + if(zed_) + { + sl::zed::SENSING_MODE dm_type = sl::zed::RAW; + bool res = zed_->grab(dm_type); + + if(!res) + { + // get left image + cv::Mat rgbaLeft = slMat2cvMat(zed_->retrieveImage(static_cast (sl::zed::STEREO_LEFT))); + cv::Mat left; + cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR); + + if(rgbdMode_) + { + // get depth image + cv::Mat depth; + slMat2cvMat(zed_->retrieveMeasure(sl::zed::MEASURE::DEPTH)).copyTo(depth); + depth /= 1000.0; + + data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now()); + } + else + { + // get right image + cv::Mat rgbaRight = slMat2cvMat(zed_->retrieveImage(static_cast (sl::zed::STEREO_RIGHT))); + cv::Mat right; + cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY); + + data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now()); + } + } + else + { + UERROR("CameraStereoZed: Failed to grab images!"); + } + } +#else + UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!"); +#endif + return data; +} + + // // CameraStereoImages // @@ -921,7 +1071,21 @@ CameraStereoVideo::CameraStereoVideo( const Transform & localTransform) : Camera(imageRate, localTransform), path_(path), - rectifyImages_(rectifyImages) + rectifyImages_(rectifyImages), + src_(CameraVideo::kVideoFile), + usbDevice_(0) +{ +} + +CameraStereoVideo::CameraStereoVideo( + int device, + float imageRate, + const Transform & localTransform) : + Camera(imageRate, localTransform), + path_(""), + rectifyImages_(false), + src_(CameraVideo::kUsbDevice), + usbDevice_(device) { } @@ -932,29 +1096,51 @@ CameraStereoVideo::~CameraStereoVideo() bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName) { + cameraName_ = cameraName; if(capture_.isOpened()) { capture_.release(); } - ULOGGER_DEBUG("Camera: filename=\"%s\"", path_.c_str()); - capture_.open(path_.c_str()); + + if (src_ == CameraVideo::kUsbDevice) + { + ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_); + capture_.open(usbDevice_); + } + else if (src_ == CameraVideo::kVideoFile) + { + ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str()); + capture_.open(path_.c_str()); + } + else + { + ULOGGER_ERROR("CameraStereoVideo: Unknown source..."); + } if(!capture_.isOpened()) { - ULOGGER_ERROR("Camera: Failed to create a capture object!"); + ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!"); capture_.release(); return false; } else { - // look for calibration files - cameraName_ = cameraName; - if(!calibrationFolder.empty() && !cameraName.empty()) + if (cameraName_.empty()) { - if(!stereoModel_.load(calibrationFolder, cameraName)) + unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID); + if (guid != 0 && guid != 0xffffffff) + { + cameraName_ = uFormat("%08x", guid); + } + } + + // look for calibration files + if(!calibrationFolder.empty() && !cameraName_.empty()) + { + if(!stereoModel_.load(calibrationFolder, cameraName_)) { UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", - cameraName.c_str(), calibrationFolder.c_str()); + cameraName_.c_str(), calibrationFolder.c_str()); } else { @@ -1007,7 +1193,7 @@ SensorData CameraStereoVideo::captureImage() rightCvt = true; } - if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification()) + if((src_ != CameraVideo::kVideoFile || rectifyImages_) && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification()) { leftImage = stereoModel_.left().rectifyImage(leftImage); rightImage = stereoModel_.right().rectifyImage(rightImage); diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index bab6251f..6235e731 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -3425,7 +3425,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size()); unsigned int i=0; float decimationRatio = preDecimation / _imagePostDecimation; - double log2value = log(preDecimation)/log(2); + double log2value = log(double(preDecimation))/log(2.0); for(std::list::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i) { cv::KeyPoint kpt = keypoints[i]; diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index d438b7cb..d2663965 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -253,7 +253,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info) // transform back the keypoints in the original image std::vector kpts = decimatedData.keypoints(); - double log2value = log(_imageDecimation)/log(2); + double log2value = log(double(_imageDecimation))/log(2.0); for(unsigned int i=0; i #include #include +#include #include #include #include @@ -724,12 +725,11 @@ void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextIm cv::Mat disparityFromStereoImages( const cv::Mat & leftImage, const cv::Mat & rightImage, - int type) + const ParametersMap & parameters) { UASSERT(!leftImage.empty() && !rightImage.empty()); UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows); UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1); - UASSERT(type == CV_32FC1 || type == CV_16SC1); cv::Mat leftMono; if(leftImage.channels() == 3) @@ -741,32 +741,9 @@ cv::Mat disparityFromStereoImages( leftMono = leftImage; } cv::Mat disparity; -#if CV_MAJOR_VERSION < 3 - cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET); - stereo.state->SADWindowSize = 15; - stereo.state->minDisparity = 0; - stereo.state->numberOfDisparities = 64; - stereo.state->preFilterSize = 9; - stereo.state->preFilterCap = 31; - stereo.state->uniquenessRatio = 15; - stereo.state->textureThreshold = 10; - stereo.state->speckleWindowSize = 100; - stereo.state->speckleRange = 4; - stereo(leftMono, rightImage, disparity, type); -#else - cv::Ptr stereo = cv::StereoBM::create(); - stereo->setBlockSize(15); - stereo->setMinDisparity(0); - stereo->setNumDisparities(64); - stereo->setPreFilterSize(9); - stereo->setPreFilterCap(31); - stereo->setUniquenessRatio(15); - stereo->setTextureThreshold(10); - stereo->setSpeckleWindowSize(100); - stereo->setSpeckleRange(4); - stereo->compute(leftMono, rightImage, disparity); -#endif - return disparity; + + StereoBM stereo(parameters); + return stereo.computeDisparity(leftMono, rightImage); } cv::Mat depthFromDisparity(const cv::Mat & disparity, diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 48016e68..e4755a65 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -588,7 +588,8 @@ pcl::PointCloud::Ptr cloudFromStereoImages( int decimation, float maxDepth, float minDepth, - std::vector * validIndices) + std::vector * validIndices, + const ParametersMap & parameters) { UASSERT(!imageLeft.empty() && !imageRight.empty()); UASSERT(imageRight.type() == CV_8UC1); @@ -623,7 +624,7 @@ pcl::PointCloud::Ptr cloudFromStereoImages( return cloudFromDisparityRGB( leftColor, - util2d::disparityFromStereoImages(leftMono, rightMono), + util2d::disparityFromStereoImages(leftMono, rightMono, parameters), modelDecimation, decimation, maxDepth, @@ -636,7 +637,8 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( int decimation, float maxDepth, float minDepth, - std::vector * validIndices) + std::vector * validIndices, + const ParametersMap & parameters) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); @@ -696,7 +698,7 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudFromSensorData( leftMono = sensorData.imageRaw(); } cloud = cloudFromDisparity( - util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()), + util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters), sensorData.stereoCameraModel(), decimation, maxDepth, @@ -719,7 +721,8 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( int decimation, float maxDepth, float minDepth, - std::vector * validIndices) + std::vector * validIndices, + const ParametersMap & parameters) { UASSERT(!sensorData.imageRaw().empty()); UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) || @@ -804,7 +807,8 @@ pcl::PointCloud::Ptr RTABMAP_EXP cloudRGBFromSensorData( decimation, maxDepth, minDepth, - validIndices); + validIndices, + parameters); if(cloud->size()) { diff --git a/guilib/include/rtabmap/gui/CameraViewer.h b/guilib/include/rtabmap/gui/CameraViewer.h index 1f73680d..54b7fe5f 100644 --- a/guilib/include/rtabmap/gui/CameraViewer.h +++ b/guilib/include/rtabmap/gui/CameraViewer.h @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include class QSpinBox; class QCheckBox; @@ -47,7 +48,9 @@ class RTABMAPGUI_EXP CameraViewer : public QDialog, public UEventsHandler { Q_OBJECT public: - CameraViewer(QWidget * parent = 0); + CameraViewer( + QWidget * parent = 0, + const ParametersMap & parameters = ParametersMap()); virtual ~CameraViewer(); public slots: @@ -61,6 +64,7 @@ private: bool processingImages_; QSpinBox * decimationSpin_; int validDecimationValue_; + ParametersMap parameters_; QPushButton * pause_; QCheckBox * showCloudCheckbox_; QCheckBox * showScanCheckbox_; diff --git a/guilib/include/rtabmap/gui/LoopClosureViewer.h b/guilib/include/rtabmap/gui/LoopClosureViewer.h index f212e783..9bfff491 100644 --- a/guilib/include/rtabmap/gui/LoopClosureViewer.h +++ b/guilib/include/rtabmap/gui/LoopClosureViewer.h @@ -57,7 +57,7 @@ public slots: void setDecimation(int decimation) {decimation_ = decimation;} void setMaxDepth(int maxDepth) {maxDepth_ = maxDepth;} void setMinDepth(int minDepth) {minDepth_ = minDepth;} - void updateView(const Transform & AtoB = Transform()); + void updateView(const Transform & AtoB = Transform(), const ParametersMap & parameters = ParametersMap()); protected: virtual void showEvent(QShowEvent * event); diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 737176df..3f512daa 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -153,6 +153,8 @@ private slots: void selectFreenect2(); void selectStereoDC1394(); void selectStereoFlyCapture2(); + void selectStereoZed(); + void selectStereoUsb(); void dumpTheMemory(); void dumpThePrediction(); void sendGoal(); diff --git a/guilib/include/rtabmap/gui/OdometryViewer.h b/guilib/include/rtabmap/gui/OdometryViewer.h index 7cbe1597..5fe1a52d 100644 --- a/guilib/include/rtabmap/gui/OdometryViewer.h +++ b/guilib/include/rtabmap/gui/OdometryViewer.h @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/RtabmapGuiExp.h" // DLL export/import defines #include "rtabmap/core/OdometryEvent.h" +#include "rtabmap/core/Parameters.h" #include #include "rtabmap/utilite/UEventsHandler.h" @@ -49,7 +50,14 @@ class RTABMAPGUI_EXP OdometryViewer : public QDialog, public UEventsHandler Q_OBJECT public: - OdometryViewer(int maxClouds = 10, int decimation = 2, float voxelSize = 0.0f, float maxDepth = 0, int qualityWarningThr=0, QWidget * parent = 0); + OdometryViewer( + int maxClouds = 10, + int decimation = 2, + float voxelSize = 0.0f, + float maxDepth = 0, + int qualityWarningThr=0, + QWidget * parent = 0, + const ParametersMap & parameters = ParametersMap()); virtual ~OdometryViewer(); public slots: @@ -83,6 +91,7 @@ private: QCheckBox * featuresShown_; QLabel * timeLabel_; int validDecimationValue_; + ParametersMap parameters_; }; } /* namespace rtabmap */ diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index f9ea66be..a2cb8ef8 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -96,6 +96,8 @@ public: kSrcFlyCapture2 = 101, kSrcStereoImages = 102, kSrcStereoVideo = 103, + kSrcStereoZed = 104, + kSrcStereoUsb = 105, kSrcRGB = 200, kSrcUsbDevice = 200, diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 2deff74a..bfd29032 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -47,12 +47,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -CameraViewer::CameraViewer(QWidget * parent) : +CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : QDialog(parent), imageView_(new ImageView(this)), cloudView_(new CloudViewer(this)), processingImages_(false), - validDecimationValue_(1) + validDecimationValue_(1), + parameters_(parameters) { qRegisterMetaType("rtabmap::SensorData"); @@ -146,12 +147,12 @@ void CameraViewer::showImage(const rtabmap::SensorData & data) if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty()) { showCloudCheckbox_->setEnabled(true); - cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_)); + cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_)); } else if(!data.depthOrRightRaw().empty()) { showCloudCheckbox_->setEnabled(true); - cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_)); + cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_)); } } } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index ced59d03..4859b635 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -1509,7 +1509,7 @@ void DatabaseViewer::view3DMap() pcl::PointCloud::Ptr cloud; UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1); UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1); - cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth); + cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, 0, ui_->parameters_toolbox->getParameters()); if(cloud->size()) { @@ -1733,7 +1733,7 @@ void DatabaseViewer::generate3DMap() UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1); UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1); pcl::IndicesPtr validIndices(new std::vector); - cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, validIndices.get()); + cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, validIndices.get(), ui_->parameters_toolbox->getParameters()); if(assemble) { @@ -2275,7 +2275,7 @@ void DatabaseViewer::update(int value, } else { - cloud = util3d::cloudRGBFromSensorData(data); + cloud = util3d::cloudRGBFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters()); } if(cloud->size()) { @@ -2312,7 +2312,7 @@ void DatabaseViewer::update(int value, else { pcl::PointCloud::Ptr cloud; - cloud = util3d::cloudFromSensorData(data); + cloud = util3d::cloudFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters()); if(cloud->size()) { view3D->addCloud("0", cloud); @@ -2945,11 +2945,11 @@ void DatabaseViewer::updateConstraintView( pcl::PointCloud::Ptr cloudFrom, cloudTo; if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty()) { - cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1); + cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters()); } if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty()) { - cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1); + cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters()); } if(cloudFrom.get() && cloudFrom->size()) @@ -3346,7 +3346,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) ui_->spinBox_projDecimation->value(), ui_->doubleSpinBox_projMaxDepth->value(), ui_->doubleSpinBox_projMinDepth->value(), - validIndices.get()); + validIndices.get(), + ui_->parameters_toolbox->getParameters()); UASSERT(ui_->doubleSpinBox_gridCellSize->value() > 0); cloud = util3d::voxelize(cloud, validIndices, ui_->doubleSpinBox_gridCellSize->value()); @@ -3793,12 +3794,16 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update dataFrom, ui_->spinBox_icp_decimation->value(), ui_->doubleSpinBox_icp_maxDepth->value(), - ui_->doubleSpinBox_icp_minDepth->value()); + ui_->doubleSpinBox_icp_minDepth->value(), + 0, + ui_->parameters_toolbox->getParameters()); pcl::PointCloud::Ptr cloudTo = util3d::cloudFromSensorData( dataTo, ui_->spinBox_icp_decimation->value(), ui_->doubleSpinBox_icp_maxDepth->value(), - ui_->doubleSpinBox_icp_minDepth->value()); + ui_->doubleSpinBox_icp_minDepth->value(), + 0, + ui_->parameters_toolbox->getParameters()); int maxLaserScans = cloudFrom->size(); dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0); dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0); diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 83ac71d7..bc90a300 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -337,7 +337,8 @@ void ExportCloudsDialog::exportClouds( const std::map & mapIds, const QMap & cachedSignatures, const std::map::Ptr, pcl::IndicesPtr> > & createdClouds, - const QString & workingDirectory) + const QString & workingDirectory, + const ParametersMap & parameters) { std::map::Ptr> clouds; std::map meshes; @@ -351,6 +352,7 @@ void ExportCloudsDialog::exportClouds( cachedSignatures, createdClouds, workingDirectory, + parameters, clouds, meshes, textureMeshes)) @@ -387,7 +389,8 @@ void ExportCloudsDialog::viewClouds( const std::map & mapIds, const QMap & cachedSignatures, const std::map::Ptr, pcl::IndicesPtr> > & createdClouds, - const QString & workingDirectory) + const QString & workingDirectory, + const ParametersMap & parameters) { std::map::Ptr> clouds; std::map meshes; @@ -401,6 +404,7 @@ void ExportCloudsDialog::viewClouds( cachedSignatures, createdClouds, workingDirectory, + parameters, clouds, meshes, textureMeshes)) @@ -517,6 +521,7 @@ bool ExportCloudsDialog::getExportedClouds( const QMap & cachedSignatures, const std::map::Ptr, pcl::IndicesPtr> > & createdClouds, const QString & workingDirectory, + const ParametersMap & parameters, std::map::Ptr> & cloudsWithNormals, std::map & meshes, std::map & textureMeshes) @@ -550,7 +555,8 @@ bool ExportCloudsDialog::getExportedClouds( std::map::Ptr, pcl::IndicesPtr> > clouds = this->getClouds( poses, cachedSignatures, - createdClouds); + createdClouds, + parameters); pcl::PointCloud::Ptr rawAssembledCloud(new pcl::PointCloud); std::vector rawCameraIndices; @@ -1006,7 +1012,8 @@ bool ExportCloudsDialog::getExportedClouds( std::map::Ptr, pcl::IndicesPtr> > ExportCloudsDialog::getClouds( const std::map & poses, const QMap & cachedSignatures, - const std::map::Ptr, pcl::IndicesPtr> > & createdClouds) const + const std::map::Ptr, pcl::IndicesPtr> > & createdClouds, + const ParametersMap & parameters) const { std::map::Ptr, pcl::IndicesPtr> > clouds; int i=0; @@ -1038,7 +1045,8 @@ std::map::Ptr, pcl::Indic _ui->spinBox_decimation->value(), _ui->doubleSpinBox_maxDepth->value(), _ui->doubleSpinBox_minDepth->value(), - indices.get()); + indices.get(), + parameters); // Don't voxelize if we create organized mesh if(!(_ui->comboBox_pipeline->currentIndex()==0 && _ui->groupBox_meshing->isChecked())) diff --git a/guilib/src/ExportCloudsDialog.h b/guilib/src/ExportCloudsDialog.h index 7448806e..2ee93248 100644 --- a/guilib/src/ExportCloudsDialog.h +++ b/guilib/src/ExportCloudsDialog.h @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include @@ -63,14 +64,16 @@ public: const std::map & mapIds, const QMap & cachedSignatures, const std::map::Ptr, pcl::IndicesPtr> > & createdClouds, - const QString & workingDirectory); + const QString & workingDirectory, + const ParametersMap & parameters); void viewClouds( const std::map & poses, const std::map & mapIds, const QMap & cachedSignatures, const std::map::Ptr, pcl::IndicesPtr> > & createdClouds, - const QString & workingDirectory); + const QString & workingDirectory, + const ParametersMap & parameters); signals: void configChanged(); @@ -86,13 +89,15 @@ private: std::map::Ptr, pcl::IndicesPtr> > getClouds( const std::map & poses, const QMap & cachedSignatures, - const std::map::Ptr, pcl::IndicesPtr> > & createdClouds) const; + const std::map::Ptr, pcl::IndicesPtr> > & createdClouds, + const ParametersMap & parameters) const; bool getExportedClouds( const std::map & poses, const std::map & mapIds, const QMap & cachedSignatures, const std::map::Ptr, pcl::IndicesPtr> > & createdClouds, const QString & workingDirectory, + const ParametersMap & parameters, std::map::Ptr> & clouds, std::map & meshes, std::map & textureMeshes); diff --git a/guilib/src/GuiLib.qrc b/guilib/src/GuiLib.qrc index 103266f7..9671ac55 100644 --- a/guilib/src/GuiLib.qrc +++ b/guilib/src/GuiLib.qrc @@ -29,6 +29,7 @@ images/sense.png images/xtion_pro_live.png images/bumblebee2.png - images/webcam.png + images/webcam.png + images/zed.png diff --git a/guilib/src/LoopClosureViewer.cpp b/guilib/src/LoopClosureViewer.cpp index e04b2690..22f675a5 100644 --- a/guilib/src/LoopClosureViewer.cpp +++ b/guilib/src/LoopClosureViewer.cpp @@ -67,7 +67,7 @@ void LoopClosureViewer::setData(const Signature & sA, const Signature & sB) } } -void LoopClosureViewer::updateView(const Transform & transform) +void LoopClosureViewer::updateView(const Transform & transform, const ParametersMap & parameters) { if(sA_.id()>0 && sB_.id()>0) { @@ -106,8 +106,8 @@ void LoopClosureViewer::updateView(const Transform & transform) { //cloud 3d pcl::PointCloud::Ptr cloudA, cloudB; - cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, minDepth); - cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, minDepth); + cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, minDepth, 0, parameters); + cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, minDepth, 0, parameters); //cloud 2d pcl::PointCloud::Ptr scanA, scanB; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 2bce2a1a..20a964c0 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -399,6 +399,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2())); connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394())); connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2())); + connect(_ui->actionStereoZed, SIGNAL(triggered()), this, SLOT(selectStereoZed())); + connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb())); _ui->actionFreenect->setEnabled(CameraFreenect::available()); _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available()); _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available()); @@ -408,6 +410,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->actionFreenect2->setEnabled(CameraFreenect2::available()); _ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available()); _ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available()); + _ui->actionStereoZed->setEnabled(CameraStereoZed::available()); this->updateSelectSourceMenu(); connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences())); @@ -884,7 +887,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom) _preferencesDialog->getCloudDecimation(1), _preferencesDialog->getCloudMaxDepth(1), _preferencesDialog->getCloudMinDepth(1), - indices.get()); + indices.get(), + _preferencesDialog->getAllParameters()); if(indices->size()) { cloud = util3d::transformPointCloud(cloud, pose); @@ -1587,7 +1591,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) if(_ui->dockWidget_loopClosureViewer->isVisible()) { UTimer loopTimer; - _loopClosureViewer->updateView(); + _loopClosureViewer->updateView(Transform(), _preferencesDialog->getAllParameters()); UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed()); _ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f)); } @@ -2219,7 +2223,8 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int _preferencesDialog->getCloudDecimation(0), _preferencesDialog->getCloudMaxDepth(0), _preferencesDialog->getCloudMinDepth(0), - indices.get()); + indices.get(), + _preferencesDialog->getAllParameters()); //compute normals pcl::PointCloud::Ptr cloud = util3d::computeNormals(cloudWithoutNormals, 10); @@ -2560,7 +2565,7 @@ Transform MainWindow::alignPosesToGroundTruth( Transform t = Transform::getIdentity(); if(groundTruth.size() && poses.size()) { - unsigned int maxSize = poses.size()>groundTruth.size()?poses.size():groundTruth.size(); + unsigned int maxSize = poses.size()>groundTruth.size()? (unsigned int)poses.size(): (unsigned int)groundTruth.size(); pcl::PointCloud cloud1, cloud2; cloud1.resize(maxSize); cloud2.resize(maxSize); @@ -3240,7 +3245,10 @@ void MainWindow::updateSelectSourceMenu() _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDatabase || _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages || _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcVideo || - _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages); + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoImages || + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoVideo || + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRGBDImages + ); _ui->actionOpenNI_PCL->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL); _ui->actionOpenNI_PCL_ASUS->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcOpenNI_PCL); @@ -3253,6 +3261,8 @@ void MainWindow::updateSelectSourceMenu() _ui->actionFreenect2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFreenect2); _ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394); _ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2); + _ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed); + _ui->actionStereoUsb->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoUsb); } void MainWindow::changeImgRateSetting() @@ -4449,7 +4459,15 @@ void MainWindow::selectStereoFlyCapture2() { _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcFlyCapture2); } +void MainWindow::selectStereoZed() +{ + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoZed); +} +void MainWindow::selectStereoUsb() +{ + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoUsb); +} void MainWindow::dumpTheMemory() { @@ -5089,7 +5107,8 @@ void MainWindow::exportClouds() _currentMapIds, _cachedSignatures, _createdClouds, - _preferencesDialog->getWorkingDirectory()); + _preferencesDialog->getWorkingDirectory(), + _preferencesDialog->getAllParameters()); } void MainWindow::viewClouds() @@ -5104,7 +5123,8 @@ void MainWindow::viewClouds() _currentMapIds, _cachedSignatures, _createdClouds, - _preferencesDialog->getWorkingDirectory()); + _preferencesDialog->getWorkingDirectory(), + _preferencesDialog->getAllParameters()); } diff --git a/guilib/src/OdometryViewer.cpp b/guilib/src/OdometryViewer.cpp index 67dc2182..0723b06e 100644 --- a/guilib/src/OdometryViewer.cpp +++ b/guilib/src/OdometryViewer.cpp @@ -49,7 +49,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap { -OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, float maxDepth, int qualityWarningThr, QWidget * parent) : +OdometryViewer::OdometryViewer( + int maxClouds, + int decimation, + float voxelSize, + float maxDepth, + int qualityWarningThr, + QWidget * parent, + const ParametersMap & parameters) : QDialog(parent), imageView_(new ImageView(this)), cloudView_(new CloudViewer(this)), @@ -59,7 +66,8 @@ OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, f lastOdomPose_(Transform::getIdentity()), qualityWarningThr_(qualityWarningThr), id_(0), - validDecimationValue_(1) + validDecimationValue_(1), + parameters_(parameters) { qRegisterMetaType("rtabmap::OdometryEvent"); @@ -241,7 +249,8 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom) validDecimationValue_, 0, 0, - validIndices.get()); + validIndices.get(), + parameters_); if(voxelSpin_->value()) { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index b914672c..0c272dc6 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -237,6 +237,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : { _ui->comboBox_cameraStereo->setItemData(1, 0, Qt::UserRole - 1); } + if (!CameraStereoZed::available()) + { + _ui->comboBox_cameraRGBD->setItemData(7, 0, Qt::UserRole - 1); + _ui->comboBox_cameraStereo->setItemData(4, 0, Qt::UserRole - 1); + } _ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable()); _ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable()); @@ -466,6 +471,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->lineEdit_cameraStereoVideo_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_stereoVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_stereoZed_computeDisparity, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -1271,6 +1278,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_stereoImages_rectify->setChecked(false); _ui->lineEdit_cameraStereoVideo_path->setText(""); _ui->checkBox_stereoVideo_rectify->setChecked(false); + _ui->checkBox_stereoZed_computeDisparity->setChecked(true); _ui->checkBox_cameraImages_timestamps->setChecked(false); _ui->checkBox_cameraImages_syncTimeStamps->setChecked(true); @@ -1595,6 +1603,11 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->checkBox_stereoVideo_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereoVideo_rectify->isChecked()).toBool()); settings.endGroup(); // StereoVideo + settings.beginGroup("StereoZed"); + _ui->checkBox_stereoZed_computeDisparity->setChecked(settings.value("compute_disp", _ui->checkBox_stereoZed_computeDisparity->isChecked()).toBool()); + settings.endGroup(); // StereoZed + + settings.beginGroup("Images"); _ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString()); _ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt()); @@ -1979,6 +1992,12 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("rectify", _ui->checkBox_stereoVideo_rectify->isChecked()); settings.endGroup(); // StereoVideo + settings.beginGroup("StereoZed"); + settings.setValue("compute_disp", _ui->checkBox_stereoZed_computeDisparity->isChecked()); + settings.endGroup(); // StereoZed + + + settings.beginGroup("Images"); settings.setValue("path", _ui->source_images_lineEdit_path->text()); settings.setValue("startPos", _ui->source_images_spinBox_startPos->value()); @@ -2556,17 +2575,16 @@ void PreferencesDialog::selectSourceDriver(Src src) else if(src >= kSrcStereo && srccomboBox_sourceType->setCurrentIndex(1); - _ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcStereo); + _ui->comboBox_cameraStereo->setCurrentIndex(src - kSrcStereo); } else if(src >= kSrcRGB && srccomboBox_sourceType->setCurrentIndex(2); - _ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcRGB); + _ui->source_comboBox_image_type->setCurrentIndex(src - kSrcRGB); } else if(src >= kSrcDatabase) { _ui->comboBox_sourceType->setCurrentIndex(3); - _ui->comboBox_cameraRGBD->setCurrentIndex(src - kSrcDatabase); } if(validateForm()) @@ -3427,8 +3445,13 @@ void PreferencesDialog::updateSourceGrpVisibility() _ui->groupBox_cameraRGBDImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD); _ui->groupBox_openni->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI_PCL-kSrcRGBD); - _ui->stackedWidget_stereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && (_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo-kSrcStereo || _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo)); + _ui->stackedWidget_stereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && + (_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo-kSrcStereo || + _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo || + _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo)); _ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo); + _ui->groupBox_cameraStereoVideo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo - kSrcStereo); + _ui->groupBox_cameraStereoZed->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo); _ui->stackedWidget_image->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && (_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB || _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB)); _ui->source_groupBox_images->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB); @@ -3948,6 +3971,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) _ui->lineEdit_cameraImages_timestamps->text().toStdString(), _ui->checkBox_cameraImages_syncTimeStamps->isChecked()); } + else if (driver == kSrcStereoUsb) + { + camera = new CameraStereoVideo( + this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } else if(driver == kSrcStereoVideo) { camera = new CameraStereoVideo( @@ -3956,6 +3986,13 @@ Camera * PreferencesDialog::createCamera(bool useRawImages) this->getGeneralInputRate(), this->getSourceLocalTransform()); } + else if (driver == kSrcStereoZed) + { + camera = new CameraStereoZed( + _ui->checkBox_stereoZed_computeDisparity->isChecked(), + this->getGeneralInputRate(), + this->getSourceLocalTransform()); + } else if(driver == kSrcUsbDevice) { camera = new CameraVideo( @@ -4219,7 +4256,8 @@ void PreferencesDialog::testOdometry() 0.0f, _ui->doubleSpinBox_maxDepth_odom->value(), this->getOdomQualityWarnThr(), - this); + this, + this->getAllParameters()); odomViewer->setWindowTitle(tr("Odometry viewer")); odomViewer->resize(1280, 480+QPushButton().minimumHeight()); odomViewer->registerToEventsManager(); @@ -4269,7 +4307,7 @@ void PreferencesDialog::testOdometry() void PreferencesDialog::testCamera() { - CameraViewer * window = new CameraViewer(this); + CameraViewer * window = new CameraViewer(this, this->getAllParameters()); window->setWindowTitle(tr("Camera viewer")); window->resize(1280, 480+QPushButton().minimumHeight()); window->registerToEventsManager(); diff --git a/guilib/src/images/zed.png b/guilib/src/images/zed.png new file mode 100644 index 00000000..8bfc30f4 Binary files /dev/null and b/guilib/src/images/zed.png differ diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 42227ddb..5f3d875f 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -7,7 +7,7 @@ 0 0 1012 - 711 + 712 @@ -27,7 +27,7 @@ 0 0 1012 - 25 + 21 @@ -190,7 +190,19 @@ + + + Zed camera + + + + :/images/zed.png:/images/zed.png + + + + + @@ -1325,7 +1337,7 @@ true - More options... + More Options... @@ -1391,6 +1403,22 @@ Anchor clouds to ground truth + + + true + + + Zed sdk + + + + + true + + + Stereo Usb Camera + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 64d368da..b0743326 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -64,15 +64,24 @@ 0 0 - 686 - 2023 + 685 + 1826 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -86,7 +95,7 @@ QFrame::Raised - 11 + 3 @@ -2006,7 +2015,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 3 + 1 @@ -2182,7 +2191,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 20 - 306 + 0 @@ -2199,7 +2208,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 20 - 306 + 0 @@ -2216,7 +2225,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 20 - 306 + 0 @@ -2730,7 +2739,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Grabber for stereo devices (i.e., Bumblebee2). + Grabber for stereo devices (i.e., Bumblebee2, Zed camera). true @@ -2764,6 +2773,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Video (Side-by-Side) + + + ZED sdk + + + + + Usb camera (Side-by-Side) + + @@ -2798,7 +2817,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 3 + 5 @@ -2810,7 +2829,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 20 - 306 + 0 @@ -2827,7 +2846,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 20 - 306 + 0 @@ -2943,7 +2962,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + 0 @@ -3006,6 +3025,79 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + 0 + 0 + + + + Zed sdk + + + + + + + + + + + + + + + Compute disparity with the GPU (using Zed sdk approach). Note that "Generate disparity image..." above will be ignored if set. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 217 + + + + + + @@ -3069,7 +3161,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 1 + 0 @@ -3422,7 +3514,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - + + 0 + + + 0 + + + 0 + + 0 @@ -8579,7 +8680,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + 0 + + + 0 + + + 0 + + 0 @@ -8719,7 +8829,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - + + 0 + + + 0 + + + 0 + + 0 @@ -8877,7 +8996,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -8957,7 +9085,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 @@ -9069,7 +9206,16 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - + + 0 + + + 0 + + + 0 + + 0 diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index c7739057..f94dc3a0 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -53,6 +53,7 @@ void showUsage() " 5=Freenect2 (Kinect v2)\n" " 6=DC1394 (Bumblebee2)\n" " 7=FlyCapture2 (Bumblebee2)\n" + " 8=ZED stereo\n" " Options:\n" " -rate #.# Input rate Hz (default 0=inf)\n" " -save_stereo \"path\" Save stereo images in a folder or a video file (side by side *.avi).\n" @@ -149,9 +150,9 @@ int main(int argc, char * argv[]) // last driver = atoi(argv[i]); - if(driver < 0 || driver > 7) + if(driver < 0 || driver > 8) { - UERROR("driver should be between 0 and 6."); + UERROR("driver should be between 0 and 8."); showUsage(); } } @@ -235,6 +236,15 @@ int main(int argc, char * argv[]) } camera = new rtabmap::CameraStereoFlyCapture2(); } + else if(driver == 8) + { + if(!rtabmap::CameraStereoZed::available()) + { + UERROR("Not built with ZED sdk support..."); + exit(-1); + } + camera = new rtabmap::CameraStereoZed(true); + } else { UFATAL("");