diff --git a/CMakeLists.txt b/CMakeLists.txt index 688a906119..8d12f9aa4e 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 21) -SET(RTABMAP_PATCH_VERSION 4) +SET(RTABMAP_PATCH_VERSION 5) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/app/android/jni/CameraARCore.cpp b/app/android/jni/CameraARCore.cpp index 526c177926..717b5ba511 100644 --- a/app/android/jni/CameraARCore.cpp +++ b/app/android/jni/CameraARCore.cpp @@ -334,11 +334,12 @@ void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayR } } -SensorData CameraARCore::captureImage(CameraInfo * info) +SensorData CameraARCore::updateDataOnRender(Transform & pose) { UScopeMutex lock(arSessionMutex_); //LOGI("Capturing image..."); + pose.setNull(); SensorData data; if(!arSession_) { @@ -370,7 +371,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info) if (geometry_changed != 0 || !uvs_initialized_) { ArFrame_transformCoordinates2d( arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES, - BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED, + BackgroundRenderer::kNumVertices, BackgroundRenderer_kVerticesDevice, AR_COORDINATES_2D_TEXTURE_NORMALIZED, transformed_uvs_); UASSERT(transformed_uvs_); uvs_initialized_ = true; @@ -393,7 +394,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info) ArTrackingState camera_tracking_state; ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); - Transform pose; CameraModel model; if(camera_tracking_state == AR_TRACKING_STATE_TRACKING) { @@ -401,24 +401,13 @@ SensorData CameraARCore::captureImage(CameraInfo * info) float pose_raw[7]; ArCamera_getPose(arSession_, ar_camera, arPose_); ArPose_getPoseRaw(arSession_, arPose_, pose_raw); - pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); - pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; + Transform poseArCore = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); + poseArCore = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - Transform poseArCore = pose; - if(pose.isNull()) + if(poseArCore.isNull()) { LOGE("CameraARCore: Pose is null"); } - else - { - this->poseReceived(pose); - // adjust origin - if(!getOriginOffset().isNull()) - { - pose = getOriginOffset() * pose; - } - info->odomPose = pose; - } // Get calibration parameters float fx,fy, cx, cy; @@ -551,6 +540,17 @@ SensorData CameraARCore::captureImage(CameraInfo * info) data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp); data.setFeatures(kpts, kpts3, cv::Mat()); + + if(!poseArCore.isNull()) + { + pose = poseArCore; + this->poseReceived(pose, stamp); + // adjust origin + if(!getOriginOffset().isNull()) + { + pose = getOriginOffset() * pose; + } + } } } else @@ -571,134 +571,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info) ArCamera_release(ar_camera); return data; - -} - -void CameraARCore::capturePoseOnly() -{ - UScopeMutex lock(arSessionMutex_); - //LOGI("Capturing image..."); - - if(!arSession_) - { - return; - } - - if(textureId_ != 0) - { - glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_); - glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); - glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); - glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST); - glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST); - ArSession_setCameraTextureName(arSession_, textureId_); - } - - // Update session to get current frame and render camera background. - if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) { - LOGE("CameraARCore::capturePoseOnly() ArSession_update error"); - return; - } - - // If display rotation changed (also includes view size change), we need to - // re-query the uv coordinates for the on-screen portion of the camera image. - int32_t geometry_changed = 0; - ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed); - if (geometry_changed != 0 || !uvs_initialized_) { - ArFrame_transformCoordinates2d( - arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES, - BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED, - transformed_uvs_); - UASSERT(transformed_uvs_); - uvs_initialized_ = true; - } - - ArCamera* ar_camera; - ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera); - - ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_)); - ArCamera_getProjectionMatrix(arSession_, ar_camera, - /*near=*/0.1f, /*far=*/100.f, - glm::value_ptr(projectionMatrix_)); - - // adjust origin - if(!getOriginOffset().isNull()) - { - viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); - } - - ArTrackingState camera_tracking_state; - ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); - - Transform pose; - CameraModel model; - if(camera_tracking_state == AR_TRACKING_STATE_TRACKING) - { - // pose in OpenGL coordinates - float pose_raw[7]; - ArCamera_getPose(arSession_, ar_camera, arPose_); - ArPose_getPoseRaw(arSession_, arPose_, pose_raw); - pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); - if(!pose.isNull()) - { - pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - this->poseReceived(pose); - - if(!getOriginOffset().isNull()) - { - pose = getOriginOffset() * pose; - } - } - - int32_t is_depth_supported = 0; - ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported); - - if(is_depth_supported) - { - LOGD("Acquire depth image!"); - ArImage * depthImage = nullptr; - ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage); - - ArImageFormat format; - ArImage_getFormat(arSession_, depthImage, &format); - if(format == AR_IMAGE_FORMAT_DEPTH16) - { - LOGD("Depth format detected!"); - int planeCount; - ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount); - LOGD("planeCount=%d", planeCount); - UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str()); - const uint8_t *data = nullptr; - int len = 0; - int stride; - int width; - int height; - ArImage_getWidth(arSession_, depthImage, &width); - ArImage_getHeight(arSession_, depthImage, &height); - ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride); - ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len); - - LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride); - - cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone(); - - float fx,fy, cx, cy; - int32_t rgb_width, rgb_height; - ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_); - ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy); - ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy); - ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height); - - float scaleX = (float)width / (float)rgb_width; - float scaleY = (float)height / (float)rgb_height; - CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height)); - this->setOcclusionImage(occlusionImage, occlusionModel); - } - ArImage_release(depthImage); - } - } - - ArCamera_release(ar_camera); } } /* namespace rtabmap */ diff --git a/app/android/jni/CameraARCore.h b/app/android/jni/CameraARCore.h index ead1791d5c..429fa3fc6c 100644 --- a/app/android/jni/CameraARCore.h +++ b/app/android/jni/CameraARCore.h @@ -63,23 +63,14 @@ class CameraARCore : public CameraMobile { CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false); virtual ~CameraARCore(); - bool uvsInitialized() const {return uvs_initialized_;} - const float* uvsTransformed() const {return transformed_uvs_;} - void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;} - virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height); virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); - void setupGL(); - virtual void close(); // close Tango connection + virtual void close(); // close ARCore connection virtual std::string getSerial() const; - GLuint getTextureId() const {return textureId_;} - - void imageCallback(AImageReader *reader); protected: - virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread - virtual void capturePoseOnly(); + virtual SensorData updateDataOnRender(Transform & pose); // should be called in opengl thread private: rtabmap::Transform getPoseAtTimestamp(double timestamp); diff --git a/app/android/jni/CameraAREngine.cpp b/app/android/jni/CameraAREngine.cpp index 2d1cd88c38..5d10ecf917 100644 --- a/app/android/jni/CameraAREngine.cpp +++ b/app/android/jni/CameraAREngine.cpp @@ -117,9 +117,6 @@ bool CameraAREngine::init(const std::string & calibrationFolder, const std::stri deviceTColorCamera_ = opticalRotation; - // Required as ArSession_update does some off-screen OpenGL stuff... - HwArSession_setCameraTextureName(arSession_, textureId_); - if (HwArSession_resume(arSession_) != HWAR_SUCCESS) { UERROR("Cannot resume camera!"); @@ -169,38 +166,87 @@ void CameraAREngine::close() CameraMobile::close(); } -SensorData CameraAREngine::captureImage(CameraInfo * info) +void CameraAREngine::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) +{ + CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height); + if(arSession_) + { + int ret = static_cast(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation + if (ret > 3) { + ret -= 4; + } + + HwArSession_setDisplayGeometry(arSession_, ret, width, height); + } +} + +SensorData CameraAREngine::updateDataOnRender(Transform & pose) { UScopeMutex lock(arSessionMutex_); //LOGI("Capturing image..."); + pose.setNull(); SensorData data; if(!arSession_) { return data; } + if(textureId_ == 0) + { + glGenTextures(1, &textureId_); + glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_); + glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + } + if(textureId_!=0) + HwArSession_setCameraTextureName(arSession_, textureId_); + // Update session to get current frame and render camera background. if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) { LOGE("CameraAREngine::captureImage() ArSession_update error"); return data; } + // If display rotation changed (also includes view size change), we need to + // re-query the uv coordinates for the on-screen portion of the camera image. + int32_t geometry_changed = 0; + HwArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed); + if (geometry_changed != 0 || !uvs_initialized_) { + HwArFrame_transformDisplayUvCoords( + arSession_, arFrame_, + BackgroundRenderer::kNumVertices*2, BackgroundRenderer_kVerticesView, + transformed_uvs_); + UERROR("uv: (%f,%f) (%f,%f) (%f,%f) (%f,%f)", + transformed_uvs_[0], transformed_uvs_[1], + transformed_uvs_[2], transformed_uvs_[3], + transformed_uvs_[4], transformed_uvs_[5], + transformed_uvs_[6], transformed_uvs_[7]); + UASSERT(transformed_uvs_); + uvs_initialized_ = true; + } + HwArCamera* ar_camera; HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera); + HwArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_)); + HwArCamera_getProjectionMatrix(arSession_, ar_camera, + /*near=*/0.1f, /*far=*/100.f, + glm::value_ptr(projectionMatrix_)); + + // adjust origin + if(!getOriginOffset().isNull()) + { + viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); + } + HwArTrackingState camera_tracking_state; HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); - Transform pose; if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING) { - // pose in OpenGL coordinates - float pose_raw[7]; - HwArCamera_getPose(arSession_, ar_camera, arPose_); - HwArPose_getPoseRaw(arSession_, arPose_, pose_raw); - pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); - // Get calibration parameters // FIXME: Hard-coded as getting intrinsics with the api fails float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744; @@ -274,6 +320,26 @@ SensorData CameraAREngine::captureImage(CameraInfo * info) double stamp = double(timestamp_ns)/10e8; CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight)); data = SensorData(outputRGB, outputDepth, model, 0, stamp); + + // pose in OpenGL coordinates + float pose_raw[7]; + HwArCamera_getPose(arSession_, ar_camera, arPose_); + HwArPose_getPoseRaw(arSession_, arPose_, pose_raw); + pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); + if(pose.isNull()) + { + LOGE("CameraAREngine: Pose is null"); + } + else + { + pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; + this->poseReceived(pose, stamp); + // adjust origin + if(!getOriginOffset().isNull()) + { + pose = getOriginOffset() * pose; + } + } } } else @@ -291,66 +357,8 @@ SensorData CameraAREngine::captureImage(CameraInfo * info) } HwArCamera_release(ar_camera); - - if(pose.isNull()) - { - LOGE("CameraAREngine: Pose is null"); - } - else - { - pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - this->poseReceived(pose); - // adjust origin - if(!getOriginOffset().isNull()) - { - pose = getOriginOffset() * pose; - } - info->odomPose = pose; - } return data; } -void CameraAREngine::capturePoseOnly() -{ - UScopeMutex lock(arSessionMutex_); - //LOGI("Capturing image..."); - - SensorData data; - if(!arSession_) - { - return; - } - - // Update session to get current frame and render camera background. - if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) { - LOGE("CameraARCore::captureImage() ArSession_update error"); - return; - } - - HwArCamera* ar_camera; - HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera); - - HwArTrackingState camera_tracking_state; - HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); - - Transform pose; - CameraModel model; - if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING) - { - // pose in OpenGL coordinates - float pose_raw[7]; - HwArCamera_getPose(arSession_, ar_camera, arPose_); - HwArPose_getPoseRaw(arSession_, arPose_, pose_raw); - pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); - if(!pose.isNull()) - { - pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - this->poseReceived(pose); - } - } - - HwArCamera_release(ar_camera); -} - } /* namespace rtabmap */ diff --git a/app/android/jni/CameraAREngine.h b/app/android/jni/CameraAREngine.h index d8d1264d95..03f592191e 100644 --- a/app/android/jni/CameraAREngine.h +++ b/app/android/jni/CameraAREngine.h @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -48,13 +49,14 @@ class CameraAREngine : public CameraMobile { CameraAREngine(void* env, void* context, void* activity, bool smoothing = false); virtual ~CameraAREngine(); + virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height); + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); - virtual void close(); // close Tango connection + virtual void close(); // close AREngine connection virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); - virtual void capturePoseOnly(); + virtual SensorData updateDataOnRender(Transform & pose); private: rtabmap::Transform getPoseAtTimestamp(double timestamp); @@ -69,7 +71,6 @@ class CameraAREngine : public CameraMobile { HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr; HwArPose * arPose_ = nullptr; bool arInstallRequested_; - GLuint textureId_; UMutex arSessionMutex_; }; diff --git a/app/android/jni/CameraMobile.cpp b/app/android/jni/CameraMobile.cpp index e7d936cfb2..cd7c4f140e 100644 --- a/app/android/jni/CameraMobile.cpp +++ b/app/android/jni/CameraMobile.cpp @@ -55,10 +55,8 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform( CameraMobile::CameraMobile(bool smoothing) : Camera(10), deviceTColorCamera_(Transform::getIdentity()), - spinOncePreviousStamp_(0.0), textureId_(0), uvs_initialized_(false), - previousStamp_(0.0), stampEpochOffset_(0.0), smoothing_(smoothing), colorCameraToDisplayRotation_(ROTATION_0), @@ -79,13 +77,12 @@ bool CameraMobile::init(const std::string &, const std::string &) void CameraMobile::close() { - previousPose_.setNull(); - previousStamp_ = 0.0; + firstFrame_ = true; lastKnownGPS_ = GPS(); lastEnvSensors_.clear(); originOffset_ = Transform(); originUpdate_ = false; - pose_ = Transform(); + dataPose_ = Transform(); data_ = SensorData(); if(textureId_ != 0) @@ -97,35 +94,107 @@ void CameraMobile::close() void CameraMobile::resetOrigin() { - previousPose_.setNull(); - previousStamp_ = 0.0; + firstFrame_ = true; lastKnownGPS_ = GPS(); lastEnvSensors_.clear(); - pose_ = Transform(); + dataPose_ = Transform(); data_ = SensorData(); originUpdate_ = true; } -void CameraMobile::poseReceived(const Transform & pose) +bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime) +{ + pose.setNull(); + + int maxWaitTimeMs = maxWaitTime * 1000; + + // Interpolate pose + if(!poseBuffer_.empty()) + { + poseMutex_.lock(); + int waitTry = 0; + while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs) + { + poseMutex_.unlock(); + ++waitTry; + uSleep(1); + poseMutex_.lock(); + } + if(poseBuffer_.rbegin()->first < stamp) + { + if(maxWaitTimeMs > 0) + { + UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first); + } + else + { + UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first); + } + } + else + { + std::map::const_iterator iterB = poseBuffer_.lower_bound(stamp); + std::map::const_iterator iterA = iterB; + if(iterA != poseBuffer_.begin()) + { + iterA = --iterA; + } + if(iterB == poseBuffer_.end()) + { + iterB = --iterB; + } + if(iterA == iterB && stamp == iterA->first) + { + pose = iterA->second; + } + else if(stamp >= iterA->first && stamp <= iterB->first) + { + pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second); + } + else // stamp < iterA->first + { + UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first); + } + } + poseMutex_.unlock(); + } + return !pose.isNull(); +} + +void CameraMobile::poseReceived(const Transform & pose, double deviceStamp) { if(!pose.isNull()) { - // send pose of the camera (without optical rotation) - Transform p = pose*deviceTColorCamera_; + Transform p = pose; if(originUpdate_) { originOffset_ = p.translation().inverse(); originUpdate_ = false; } + + if(stampEpochOffset_ == 0.0) + { + stampEpochOffset_ = UTimer::now() - deviceStamp; + } + + double epochStamp = stampEpochOffset_ + deviceStamp; if(!originOffset_.isNull()) { - this->post(new PoseEvent(originOffset_*p)); + p = originOffset_*p; } - else + { - this->post(new PoseEvent(p)); + UScopeMutex lock(poseMutex_); + poseBuffer_.insert(poseBuffer_.end(), std::make_pair(epochStamp, p)); + if(poseBuffer_.size() > 1000) + { + poseBuffer_.erase(poseBuffer_.begin()); + } } + + // send pose of the camera (with optical rotation) + this->post(new PoseEvent(p * deviceTColorCamera_)); } } @@ -139,11 +208,20 @@ void CameraMobile::setGPS(const GPS & gps) lastKnownGPS_ = gps; } -void CameraMobile::setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord) +void CameraMobile::addEnvSensor(int type, float value) +{ + lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value))); +} + +void CameraMobile::update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord) { - LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp()); + UScopeMutex lock(dataMutex_); + + bool notify = !data_.isValid(); + + LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp()); data_ = data; - pose_ = pose; + dataPose_ = pose; viewMatrix_ = viewMatrix; projectionMatrix_ = projectionMatrix; @@ -151,7 +229,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons // adjust origin if(!originOffset_.isNull()) { - pose_ = originOffset_ * pose_; + dataPose_ = originOffset_ * dataPose_; viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); } @@ -166,7 +244,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons uvs_initialized_ = true; } - LOGD("CameraMobile::setData textureId_=%d", (int)textureId_); + LOGD("CameraMobile::update textureId_=%d", (int)textureId_); if(textureId_ != 0 && texCoord != 0) { @@ -193,78 +271,63 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons return; } } -} -void CameraMobile::addEnvSensor(int type, float value) -{ - lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value))); + postUpdate(); + + if(notify) + { + dataReady_.release(); + } } -void CameraMobile::spinOnce() +void CameraMobile::updateOnRender() { - if(!this->isRunning()) + UScopeMutex lock(dataMutex_); + bool notify = !data_.isValid(); + + data_ = updateDataOnRender(dataPose_); + + if(data_.isValid()) { - bool ignoreFrame = false; - //float rate = 10.0f; // maximum 10 FPS for image data - double now = UTimer::now(); - /*if(rate>0.0f) - { - if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) || - ((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate)) - { - ignoreFrame = true; - } - }*/ + postUpdate(); - if(!ignoreFrame) - { - spinOnceFrameRateTimer_.start(); - spinOncePreviousStamp_ = now; - mainLoop(); - } - else + if(notify) { - // just send pose - capturePoseOnly(); + dataReady_.release(); } } } -void CameraMobile::mainLoopBegin() +SensorData CameraMobile::updateDataOnRender(Transform & pose) { - double t = cameraStartedTime_.elapsed(); - if(t < 5.0) - { - uSleep((5.0-t)*1000); // just to make sure that the camera is started - } + LOGE("To use CameraMobile::updateOnRender(), CameraMobile::updateDataOnRender() " + "should be overridden by inherited classes. Returning empty data!\n"); + return SensorData(); } -void CameraMobile::mainLoop() +void CameraMobile::postUpdate() { - CameraInfo info; - SensorData data = this->captureImage(&info); - - if(data.isValid() && !info.odomPose.isNull()) + if(data_.isValid()) { - if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0) + if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0) { - data.setGPS(lastKnownGPS_); + data_.setGPS(lastKnownGPS_); } else if(lastKnownGPS_.stamp()>0.0) { - LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp()); + LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp()); } if(lastEnvSensors_.size()) { - data.setEnvSensors(lastEnvSensors_); + data_.setEnvSensors(lastEnvSensors_); lastEnvSensors_.clear(); } - if(smoothing_ && !data.depthRaw().empty()) + if(smoothing_ && !data_.depthRaw().empty()) { //UTimer t; - data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR)); + data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR)); //LOGD("Bilateral filtering, time=%fs", t.ticks()); } @@ -273,15 +336,15 @@ void CameraMobile::mainLoop() { UDEBUG("ROTATION_90"); cv::Mat rgb, depth; - cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type()); - cv::flip(data.imageRaw(),rgb,1); + cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type()); + cv::flip(data_.imageRaw(),rgb,1); cv::transpose(rgb,rgbt); rgb = rgbt; - cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type()); - cv::flip(data.depthRaw(),depth,1); + cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type()); + cv::flip(data_.depthRaw(),depth,1); cv::transpose(depth,deptht); depth = deptht; - CameraModel model = data.cameraModels()[0]; + CameraModel model = data_.cameraModels()[0]; cv::Size sizet(model.imageHeight(), model.imageWidth()); model = CameraModel( model.fy(), @@ -290,25 +353,25 @@ void CameraMobile::mainLoop() model.cx()>0?model.imageWidth()-model.cx():0, model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0)); model.setImageSize(sizet); - data.setRGBDImage(rgb, depth, model); + data_.setRGBDImage(rgb, depth, model); - std::vector keypoints = data.keypoints(); + std::vector keypoints = data_.keypoints(); for(size_t i=0; i0?model.imageHeight()-model.cy():0, model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0)); model.setImageSize(sizet); - data.setRGBDImage(rgb, depth, model); + data_.setRGBDImage(rgb, depth, model); - std::vector keypoints = data.keypoints(); + std::vector keypoints = data_.keypoints(); for(size_t i=0; i keypoints = data.keypoints(); + std::vector keypoints = data_.keypoints(); for(size_t i=0; i(3,3) *= 0.01; - info.reg.covariance.at(4,4) *= 0.01; - info.reg.covariance.at(5,5) *= 0.01; + // linear cov = 0.0001 + info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001); + if(!firstFrame_) + { + // angular cov = 0.000001 + info->odomCovariance.at(3,3) *= 0.01; + info->odomCovariance.at(4,4) *= 0.01; + info->odomCovariance.at(5,5) *= 0.01; + } + info->odomPose = dataPose_; } - LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001); - this->post(new OdometryEvent(data, pose, info)); - previousPose_ = pose; - previousStamp_ = data.stamp(); - } - else if(!this->isKilled() && info.odomPose.isNull()) - { - LOGW("Odometry lost"); - this->post(new OdometryEvent()); - } -} -SensorData CameraMobile::captureImage(CameraInfo * info) -{ - if(info) + firstFrame_ = false; + } + else { - info->odomPose = pose_; + UWARN("CameraMobile::captureImage() invalid data!"); } - return data_; + return data; } LaserScan CameraMobile::scanFromPointCloudData( diff --git a/app/android/jni/CameraMobile.h b/app/android/jni/CameraMobile.h index 19662d7e39..8df249342b 100644 --- a/app/android/jni/CameraMobile.h +++ b/app/android/jni/CameraMobile.h @@ -68,7 +68,7 @@ class PoseEvent: public UEvent Transform pose_; }; -class CameraMobile : public Camera, public UThread, public UEventsSender { +class CameraMobile : public Camera, public UEventsSender { public: static const float bilateralFilteringSigmaS; static const float bilateralFilteringSigmaR; @@ -93,14 +93,20 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { // abstract functions virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); - virtual void close(); // inherited classes should call its parent in their close(). + virtual void close(); // inherited classes should call its parent at the end of their close(). virtual std::string getSerial() const {return "CameraMobile";} + void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord); + void updateOnRender(); + const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame void resetOrigin(); virtual bool isCalibrated() const; - void poseReceived(const Transform & pose); // in rtabmap frame + virtual bool odomProvided() const { return true; } + virtual bool getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // Return pose of device in rtabmap frame (with origin offset), stamp should be epoch time + void poseReceived(const Transform & pose, double deviceStamp); // original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch) + double getStampEpochOffset() const {return stampEpochOffset_;} const CameraModel & getCameraModel() const {return model_;} const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;} @@ -108,10 +114,7 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;} void setGPS(const GPS & gps); void addEnvSensor(int type, float value); - void setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord); - void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing - GLuint getTextureId() {return textureId_;} bool uvsInitialized() const {return uvs_initialized_;} const float* uvsTransformed() const {return transformed_uvs_;} @@ -122,17 +125,15 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; } protected: - virtual SensorData captureImage(CameraInfo * info = 0); - virtual void capturePoseOnly() {} + virtual SensorData updateDataOnRender(Transform & pose); - virtual void mainLoopBegin(); - virtual void mainLoop(); +private: + virtual SensorData captureImage(SensorCaptureInfo * info = 0); + void postUpdate(); // Should be called while being protected by dataMutex_ protected: CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame - UTimer spinOnceFrameRateTimer_; - double spinOncePreviousStamp_; GLuint textureId_; glm::mat4 viewMatrix_; @@ -141,9 +142,7 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { bool uvs_initialized_ = false; private: - Transform previousPose_; - double previousStamp_; - UTimer cameraStartedTime_; + bool firstFrame_; double stampEpochOffset_; bool smoothing_; ScreenRotation colorCameraToDisplayRotation_; @@ -152,8 +151,13 @@ class CameraMobile : public Camera, public UThread, public UEventsSender { Transform originOffset_; bool originUpdate_; + USemaphore dataReady_; + UMutex dataMutex_; SensorData data_; - Transform pose_; + Transform dataPose_; + + UMutex poseMutex_; + std::map poseBuffer_; // cv::Mat occlusionImage_; CameraModel occlusionModel_; diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index 6408588d44..5498014dea 100644 --- a/app/android/jni/CameraTango.cpp +++ b/app/android/jni/CameraTango.cpp @@ -101,7 +101,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose) if(pose->status_code == TANGO_POSE_VALID) { CameraTango* app = static_cast(context); - app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world); + app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world, pose->timestamp); } } @@ -444,7 +444,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) //LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols); UASSERT(cloud.type() == CV_32FC4); - boost::mutex::scoped_lock lock(dataMutex_); + boost::mutex::scoped_lock lock(tangoDataMutex_); // From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames // "In the current version of Project Tango Tablet RGB IR camera @@ -463,7 +463,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) if(dt >= 0.0 && dt < 0.5) { - bool notify = !data_.isValid(); + bool notify = !tangoData_.isValid(); cv::Mat tangoImage = tangoColor_; cv::Mat rgb; @@ -495,7 +495,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) else { LOGE("Not supported color format : %d.", tangoColorType); - data_ = SensorData(); + tangoData_ = SensorData(); return; } @@ -678,24 +678,24 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp) if(rawScanPublished_) { - data_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp); + tangoData_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp); } else { - data_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp); + tangoData_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp); } - data_.setGroundTruth(odom); + tangoData_.setGroundTruth(odom); } else { LOGE("Could not get depth and rgb images!?!"); - data_ = SensorData(); + tangoData_ = SensorData(); return; } if(notify) { - dataReady_.release(); + tangoDataReady_.release(); } LOGD("process cloud received %fs", timer.ticks()); } @@ -709,7 +709,7 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times { //LOGD("RGB received! %fs", timestamp); - boost::mutex::scoped_lock lock(dataMutex_); + boost::mutex::scoped_lock lock(tangoDataMutex_); tangoColor_ = tangoImage.clone(); tangoColorStamp_ = timestamp; @@ -775,10 +775,11 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp) return pose; } -SensorData CameraTango::captureImage(CameraInfo * info) +SensorData CameraTango::updateDataOnRender(Transform & pose) { //LOGI("Capturing image..."); + pose.setNull(); if(textureId_ == 0) { glGenTextures(1, &textureId_); @@ -797,10 +798,7 @@ SensorData CameraTango::captureImage(CameraInfo * info) if (status == TANGO_SUCCESS) { - if(info) - { - info->odomPose = getPoseAtTimestamp(video_overlay_timestamp); - } + pose = getPoseAtTimestamp(video_overlay_timestamp); int rotation = static_cast(getScreenRotation()) + 1; // remove 90deg camera rotation if (rotation > 3) { @@ -876,16 +874,13 @@ SensorData CameraTango::captureImage(CameraInfo * info) } SensorData data; - if(dataReady_.acquireTry(1)) + if(tangoDataReady_.acquireTry(1)) { - boost::mutex::scoped_lock lock(dataMutex_); - data = data_; - data_ = SensorData(); - if(info) - { - info->odomPose = data.groundTruth(); - data.setGroundTruth(Transform()); - } + boost::mutex::scoped_lock lock(tangoDataMutex_); + data = tangoData_; + tangoData_ = SensorData(); + pose = data.groundTruth(); + data.setGroundTruth(Transform()); } return data; diff --git a/app/android/jni/CameraTango.h b/app/android/jni/CameraTango.h index 055222f9f4..6dc0366592 100644 --- a/app/android/jni/CameraTango.h +++ b/app/android/jni/CameraTango.h @@ -52,7 +52,6 @@ class CameraTango : public CameraMobile { virtual void close(); // close Tango connection virtual std::string getSerial() const; rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const; - void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;} void setDecimation(int value) {decimation_ = value;} void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;} @@ -61,7 +60,7 @@ class CameraTango : public CameraMobile { void tangoEventReceived(int type, const char * key, const char * value); protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData updateDataOnRender(Transform & pose); private: rtabmap::Transform getPoseAtTimestamp(double timestamp); @@ -71,12 +70,12 @@ class CameraTango : public CameraMobile { bool colorCamera_; int decimation_; bool rawScanPublished_; - SensorData data_; + SensorData tangoData_; cv::Mat tangoColor_; int tangoColorType_; double tangoColorStamp_; - boost::mutex dataMutex_; - USemaphore dataReady_; + boost::mutex tangoDataMutex_; + USemaphore tangoDataReady_; cv::Mat fisheyeRectifyMapX_; cv::Mat fisheyeRectifyMapY_; }; diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 2346bb43f9..e8b0b6115c 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -65,6 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -202,6 +203,7 @@ RTABMapApp::RTABMapApp() : #endif cameraDriver_(0), camera_(0), + sensorCaptureThread_(0), rtabmapThread_(0), rtabmap_(0), logHandler_(0), @@ -216,6 +218,7 @@ RTABMapApp::RTABMapApp() : cameraColor_(true), fullResolution_(false), appendMode_(true), + useExternalLidar_(false), maxCloudDepth_(2.5), minCloudDepth_(0.0), cloudDensityLevel_(1), @@ -537,7 +540,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe // Voxelize and filter depending on the previous cloud? pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); - if(!data.imageRaw().empty() && !data.depthRaw().empty()) + if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty())) { int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows); @@ -885,7 +888,7 @@ bool RTABMapApp::startCamera() #endif LOGW("startCamera() camera driver=%d", cameraDriver_); boost::mutex::scoped_lock lock(cameraMutex_); - + if(cameraDriver_ == 0) // Tango { #ifdef RTABMAP_TANGO @@ -937,6 +940,19 @@ bool RTABMapApp::startCamera() LOGI("Start camera thread"); cameraJustInitialized_ = true; + if(useExternalLidar_) + { + rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true); + lidar->init(); + camera_->setImageRate(0); // if lidar, to get close camera synchronization + sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity()); + sensorCaptureThread_->setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, true); + } + else + { + sensorCaptureThread_ = new rtabmap::SensorCaptureThread(camera_); + } + sensorCaptureThread_->start(); return true; } UERROR("Failed camera initialization!"); @@ -948,13 +964,12 @@ void RTABMapApp::stopCamera() LOGI("stopCamera()"); { boost::mutex::scoped_lock lock(cameraMutex_); - if(camera_!=0) + if(sensorCaptureThread_!=0) { - camera_->join(true); - camera_->close(); - delete camera_; + sensorCaptureThread_->join(true); + delete sensorCaptureThread_; // camera_ is closed and deleted inside + sensorCaptureThread_ = 0; camera_ = 0; - poseBuffer_.clear(); } } { @@ -1241,7 +1256,7 @@ int RTABMapApp::Render() std::list rtabmapEvents; try { - if(camera_ == 0) + if(sensorCaptureThread_ == 0) { // We are not doing continous drawing, just measure single draw fpsTime_.restart(); @@ -1272,49 +1287,45 @@ int RTABMapApp::Render() { if(cameraDriver_ <= 2) { - camera_->spinOnce(); + camera_->updateOnRender(); } #ifdef DEBUG_RENDERING_PERFORMANCE - LOGW("Camera spinOnce %fs", time.ticks()); + LOGW("Camera updateOnRender %fs", time.ticks()); #endif - - if(cameraDriver_ != 2) + if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0) + { + main_scene_.background_renderer_ = new BackgroundRenderer(); + main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2); + } + if(camera_->uvsInitialized()) { - if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0) + uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed(); + ((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix); + if(graphOptimization_ && !mapToOdom_.isIdentity()) { - main_scene_.background_renderer_ = new BackgroundRenderer(); - main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1); + rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world; + arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix)); } - if(camera_->uvsInitialized()) + } + if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson) + { + rtabmap::CameraModel occlusionModel; + cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel); + + if(occlusionModel.isValidForProjection()) { - uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed(); - ((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix); - if(graphOptimization_ && !mapToOdom_.isIdentity()) - { - rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world; - arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix)); - } + pcl::IndicesPtr indices(new std::vector); + int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows); + pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get()); + cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform()); + occlusionMesh.cloud.reset(new pcl::PointCloud()); + pcl::copyPointCloud(*cloud, *occlusionMesh.cloud); + occlusionMesh.indices = indices; + occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_); } - if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson) + else if(!occlusionImage.empty()) { - rtabmap::CameraModel occlusionModel; - cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel); - - if(occlusionModel.isValidForProjection()) - { - pcl::IndicesPtr indices(new std::vector); - int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows); - pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get()); - cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform()); - occlusionMesh.cloud.reset(new pcl::PointCloud()); - pcl::copyPointCloud(*cloud, *occlusionMesh.cloud); - occlusionMesh.indices = indices; - occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_); - } - else if(!occlusionImage.empty()) - { - UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight()); - } + UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight()); } } #ifdef DEBUG_RENDERING_PERFORMANCE @@ -1334,14 +1345,14 @@ int RTABMapApp::Render() } } - rtabmap::OdometryEvent odomEvent; + rtabmap::SensorEvent sensorEvent; { - boost::mutex::scoped_lock lock(odomMutex_); - if(odomEvents_.size()) + boost::mutex::scoped_lock lock(sensorMutex_); + if(sensorEvents_.size()) { - LOGI("Process odom events"); - odomEvent = odomEvents_.back(); - odomEvents_.clear(); + LOGI("Process sensor events"); + sensorEvent = sensorEvents_.back(); + sensorEvents_.clear(); if(cameraJustInitialized_) { notifyCameraStarted = true; @@ -1361,7 +1372,7 @@ int RTABMapApp::Render() { main_scene_.SetCameraPose(rtabmap::opengl_world_T_rtabmap_world*pose*rtabmap::optical_T_opengl); } - if(camera_!=0 && cameraJustInitialized_) + if(sensorCaptureThread_!=0 && cameraJustInitialized_) { notifyCameraStarted = true; cameraJustInitialized_ = false; @@ -1562,9 +1573,9 @@ int RTABMapApp::Render() if(clearSceneOnNextRender_) { LOGI("Clearing all rendering data..."); - odomMutex_.lock(); - odomEvents_.clear(); - odomMutex_.unlock(); + sensorMutex_.lock(); + sensorEvents_.clear(); + sensorMutex_.unlock(); poseMutex_.lock(); poseEvents_.clear(); @@ -1800,7 +1811,7 @@ int RTABMapApp::Render() // Voxelize and filter depending on the previous cloud? pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); - if(!data.imageRaw().empty() && !data.depthRaw().empty()) + if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty())) { int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows); cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get()); @@ -2004,26 +2015,26 @@ int RTABMapApp::Render() } else { - main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && camera_!=0); + main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0); //just process the last one - if(!odomEvent.pose().isNull()) + if(!sensorEvent.info().odomPose.isNull()) { if(odomCloudShown_ && !trajectoryMode_) { - if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()) || !odomEvent.data().laserScanRaw().isEmpty()) + if((!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty()) || !sensorEvent.data().laserScanRaw().isEmpty()) { pcl::PointCloud::Ptr cloud; pcl::IndicesPtr indices(new std::vector); - if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty())) + if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty())) { - int meshDecimation = updateMeshDecimation(odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows); - cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get()); + int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows); + cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get()); } else { //scan - cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(odomEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255); + cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(sensorEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), sensorEvent.data().laserScanRaw().localTransform(), 255, 255, 255); indices->resize(cloud->size()); for(unsigned int i=0; isize(); ++i) { @@ -2034,10 +2045,10 @@ int RTABMapApp::Render() if(cloud->size() && indices->size()) { LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)", - odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows, - odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows, + sensorEvent.data().imageRaw().cols, sensorEvent.data().imageRaw().rows, + sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows, (int)cloud->width, (int)cloud->height); - main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose()); + main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*sensorEvent.info().odomPose); main_scene_.setCloudVisible(-1, true); } else @@ -2127,7 +2138,7 @@ int RTABMapApp::Render() lastPostRenderEventTime_ = UTimer::now(); - if(camera_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0) + if(sensorCaptureThread_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0) { UERROR("TangoPoseEventNotReceived"); UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6))); @@ -2319,7 +2330,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled) void RTABMapApp::setGraphOptimization(bool enabled) { graphOptimization_ = enabled; - if((camera_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0) + if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0) { std::map poses; std::multimap links; @@ -3709,19 +3720,12 @@ void RTABMapApp::postCameraPoseEvent( if(qx==0 && qy==0 && qz==0 && qw==0) { // Lost! clear buffer - poseBuffer_.clear(); camera_->resetOrigin(); // we are lost, create new session on next valid frame return; } rtabmap::Transform pose(x,y,z,qx,qy,qz,qw); pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; - camera_->poseReceived(pose); - - poseBuffer_.insert(std::make_pair(stamp, pose)); - if(poseBuffer_.size() > 1000) - { - poseBuffer_.erase(poseBuffer_.begin()); - } + camera_->poseReceived(pose, stamp); } } @@ -3833,66 +3837,41 @@ void RTABMapApp::postOdometryEvent( { pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; + rtabmap::Transform poseWithOriginOffset = pose; + if(!camera_->getOriginOffset().isNull()) + { + poseWithOriginOffset = camera_->getOriginOffset() * pose; + } + // Registration depth to rgb if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp)) { UTimer time; rtabmap::Transform motion = rtabmap::Transform::getIdentity(); - if(depthStamp != stamp && !poseBuffer_.empty()) + if(depthStamp != stamp) { // Interpolate pose - if(!poseBuffer_.empty()) + rtabmap::Transform poseDepth; + cv::Mat cov; + if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0)) + { + UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp); + } + else { - if(poseBuffer_.rbegin()->first < depthStamp) - { - UWARN("Could not find poses to interpolate at time %f (last is %f)...", depthStamp, poseBuffer_.rbegin()->first); - } - else - { - std::map::const_iterator iterB = poseBuffer_.lower_bound(depthStamp); - std::map::const_iterator iterA = iterB; - rtabmap::Transform poseDepth; - if(iterA != poseBuffer_.begin()) - { - iterA = --iterA; - } - if(iterB == poseBuffer_.end()) - { - iterB = --iterB; - } - if(iterA == iterB && depthStamp == iterA->first) - { - poseDepth = iterA->second; - } - else if(depthStamp >= iterA->first && depthStamp <= iterB->first) - { - poseDepth = iterA->second.interpolate((depthStamp-iterA->first) / (iterB->first-iterA->first), iterB->second); - } - else if(depthStamp < iterA->first) - { - UERROR("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", depthStamp, iterA->first); - } - else - { - UERROR("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", depthStamp, iterA->first, iterB->first); - } - if(!poseDepth.isNull()) - { #ifndef DISABLE_LOG - UDEBUG("poseRGB =%s (stamp=%f)", pose.prettyPrint().c_str(), depthStamp); - UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp); + UDEBUG("poseRGB =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp); + UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp); #endif - motion = pose.inverse()*poseDepth; - // transform in camera frame + motion = poseWithOriginOffset.inverse()*poseDepth; + // transform in camera frame #ifndef DISABLE_LOG - UDEBUG("motion=%s", motion.prettyPrint().c_str()); + UDEBUG("motion=%s", motion.prettyPrint().c_str()); #endif - motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation(); + motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation(); #ifndef DISABLE_LOG - UDEBUG("motion=%s", motion.prettyPrint().c_str()); + UDEBUG("motion=%s", motion.prettyPrint().c_str()); #endif - } - } } } rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame; @@ -3941,11 +3920,6 @@ void RTABMapApp::postOdometryEvent( if(!outputDepth.empty()) { - rtabmap::Transform poseWithOriginOffset = pose; - if(!camera_->getOriginOffset().isNull()) - { - poseWithOriginOffset = camera_->getOriginOffset() * pose; - } rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth())); depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform()); camera_->setOcclusionImage(outputDepth, depthModel); @@ -3971,8 +3945,7 @@ void RTABMapApp::postOdometryEvent( texCoords[5] = t5; texCoords[6] = t6; texCoords[7] = t7; - camera_->setData(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0); - camera_->spinOnce(); + camera_->update(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0); } } } @@ -3989,17 +3962,17 @@ void RTABMapApp::postOdometryEvent( bool RTABMapApp::handleEvent(UEvent * event) { - if(camera_!=0) + if(sensorCaptureThread_!=0) { // called from events manager thread, so protect the data - if(event->getClassName().compare("OdometryEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - LOGI("Received OdometryEvent!"); - if(odomMutex_.try_lock()) + LOGI("Received SensorEvent!"); + if(sensorMutex_.try_lock()) { - odomEvents_.clear(); - odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event))); - odomMutex_.unlock(); + sensorEvents_.clear(); + sensorEvents_.push_back(*((rtabmap::SensorEvent*)(event))); + sensorMutex_.unlock(); } } if(event->getClassName().compare("RtabmapEvent") == 0) diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 01fde945d8..40c9d66215 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "util.h" #include "ProgressionStatus.h" +#include #include +#include #include #include #include @@ -209,6 +211,7 @@ class RTABMapApp : public UEventsHandler { private: int cameraDriver_; rtabmap::CameraMobile * camera_; + rtabmap::SensorCaptureThread * sensorCaptureThread_; rtabmap::RtabmapThread * rtabmapThread_; rtabmap::Rtabmap * rtabmap_; rtabmap::LogHandler * logHandler_; @@ -224,6 +227,7 @@ class RTABMapApp : public UEventsHandler { bool cameraColor_; bool fullResolution_; bool appendMode_; + bool useExternalLidar_; float maxCloudDepth_; float minCloudDepth_; int cloudDensityLevel_; @@ -270,16 +274,15 @@ class RTABMapApp : public UEventsHandler { UTimer fpsTime_; std::list rtabmapEvents_; - std::list odomEvents_; + std::list sensorEvents_; std::list poseEvents_; - std::map poseBuffer_; rtabmap::Transform mapToOdom_; boost::mutex cameraMutex_; boost::mutex rtabmapMutex_; boost::mutex meshesMutex_; - boost::mutex odomMutex_; + boost::mutex sensorMutex_; boost::mutex poseMutex_; boost::mutex renderingMutex_; diff --git a/app/android/jni/background_renderer.cc b/app/android/jni/background_renderer.cc index 9908c00243..23e0fea5e9 100644 --- a/app/android/jni/background_renderer.cc +++ b/app/android/jni/background_renderer.cc @@ -155,7 +155,7 @@ void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes) } void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown) { - static_assert(std::extent::value == kNumVertices * 2, "Incorrect kVertices length"); + static_assert(std::extent::value == kNumVertices * 2, "Incorrect kVertices length"); GLuint program = shaderPrograms_[depthTexture>0?1:0]; @@ -170,7 +170,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept else #endif glBindTexture(GL_TEXTURE_2D, texture_id_); - + if(depthTexture>0) { // Texture activate unit 1 @@ -191,7 +191,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept GLuint attributeVertices = glGetAttribLocation(program, "a_Position"); GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord"); - glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices); + glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVerticesDevice); glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord); glEnableVertexAttribArray(attributeVertices); diff --git a/app/android/jni/background_renderer.h b/app/android/jni/background_renderer.h index d696a9c936..8c07aa8a96 100644 --- a/app/android/jni/background_renderer.h +++ b/app/android/jni/background_renderer.h @@ -28,9 +28,15 @@ #include "util.h" -static const GLfloat BackgroundRenderer_kVertices[] = { +static const GLfloat BackgroundRenderer_kVerticesDevice[] = { -1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f, }; +//static const GLfloat BackgroundRenderer_kVerticesView[] = { +// 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, +//}; +static const GLfloat BackgroundRenderer_kVerticesView[] = { + 0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, +}; static const GLfloat BackgroundRenderer_kTexCoord[] = { 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, }; diff --git a/app/android/libs/.gitignore b/app/android/libs/.gitignore index d392f0e82c..86d0cb2726 100644 --- a/app/android/libs/.gitignore +++ b/app/android/libs/.gitignore @@ -1 +1,4 @@ -*.jar +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java b/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java index 986fcbf6fa..456bf6160d 100644 --- a/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java +++ b/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java @@ -115,6 +115,9 @@ public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSp // Image reader that continuously processes CPU images. public TOF_ImageReader mTOFImageReader = new TOF_ImageReader(); private boolean mTOFAvailable = false; + + ByteBuffer mPreviousDepth = null; + double mPreviousDepthStamp = 0.0; public boolean isDepthSupported() {return mTOFAvailable;} @@ -757,7 +760,6 @@ public void run() { double stamp = (double)frame.getTimestamp()/10e8; if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp)); RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp); - CameraIntrinsics intrinsics = camera.getImageIntrinsics(); try{ Image image = frame.acquireCameraImage(); @@ -813,6 +815,12 @@ public void run() { depth = mTOFImageReader.depth16_raw; depthStamp = (double)mTOFImageReader.timestamp/10e8; } + + if(mPreviousDepth == null) + { + mPreviousDepth = depth; + mPreviousDepthStamp = depthStamp; + } if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f", mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp)); @@ -825,13 +833,17 @@ public void run() { rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(), depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(), stamp, - depthStamp, + depthStamp>stamp?mPreviousDepthStamp:depthStamp, y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), - depth, depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16, + depthStamp>stamp?mPreviousDepth:depth, depthStamp>stamp?mPreviousDepth.limit():depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16, points, points.limit()/4, viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0], p[0], p[5], p[8], p[9], p[10], p[11], p[14], texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]); + + + mPreviousDepthStamp = depthStamp; + mPreviousDepth = depth; } else { diff --git a/app/ios/RTABMapApp/NativeWrapper.cpp b/app/ios/RTABMapApp/NativeWrapper.cpp index dfe1259552..27d5ec2f2c 100644 --- a/app/ios/RTABMapApp/NativeWrapper.cpp +++ b/app/ios/RTABMapApp/NativeWrapper.cpp @@ -284,11 +284,11 @@ void setCameraNative(const void *object, int type) { } void postCameraPoseEventNative(const void *object, - float x, float y, float z, float qx, float qy, float qz, float qw) + float x, float y, float z, float qx, float qy, float qz, float qw, double stamp) { if(object) { - native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,0.0); + native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,stamp); } else { diff --git a/app/ios/RTABMapApp/NativeWrapper.hpp b/app/ios/RTABMapApp/NativeWrapper.hpp index 3b36a29563..7aa11d93af 100644 --- a/app/ios/RTABMapApp/NativeWrapper.hpp +++ b/app/ios/RTABMapApp/NativeWrapper.hpp @@ -67,7 +67,7 @@ bool startCameraNative(const void *object); void stopCameraNative(const void *object); void setCameraNative(const void *object, int type); void postCameraPoseEventNative(const void *object, - float x, float y, float z, float qx, float qy, float qz, float qw); + float x, float y, float z, float qx, float qy, float qz, float qw, double stamp); void postOdometryEventNative(const void *object, float x, float y, float z, float qx, float qy, float qz, float qw, float fx, float fy, float cx, float cy, diff --git a/app/ios/RTABMapApp/RTABMap.swift b/app/ios/RTABMapApp/RTABMap.swift index 2ca01745f6..dc7a602edf 100644 --- a/app/ios/RTABMapApp/RTABMap.swift +++ b/app/ios/RTABMapApp/RTABMap.swift @@ -216,18 +216,18 @@ class RTABMap { setCameraNative(native_rtabmap, Int32(type)) } - func postCameraPoseEvent(pose: simd_float4x4) { + func postCameraPoseEvent(pose: simd_float4x4, stamp: TimeInterval) { let rotation = GLKMatrix3( m: (pose[0,0], pose[0,1], pose[0,2], pose[1,0], pose[1,1], pose[1,2], pose[2,0], pose[2,1], pose[2,2])) let quat = GLKQuaternionMakeWithMatrix3(rotation) - postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w) + postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, stamp) } func notifyLost() { // a null transform will make rtabmap creating a new session - postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0) + postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0,0) } func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) { @@ -239,7 +239,7 @@ class RTABMap { let quat = GLKQuaternionMakeWithMatrix3(rotation) - postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w) + postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, frame.timestamp) let confMap = frame.sceneDepth?.confidenceMap let depthMap = frame.sceneDepth?.depthMap diff --git a/corelib/include/rtabmap/core/Camera.h b/corelib/include/rtabmap/core/Camera.h index 9c324cd786..a267f140e8 100644 --- a/corelib/include/rtabmap/core/Camera.h +++ b/corelib/include/rtabmap/core/Camera.h @@ -1,5 +1,5 @@ /* -Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without @@ -28,47 +28,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines - -#include -#include "rtabmap/core/SensorData.h" -#include "rtabmap/core/CameraInfo.h" -#include -#include -#include -#include - -class UDirectory; -class UTimer; +#include +#include namespace rtabmap { +class IMUFilter; + /** * Class Camera * */ -class RTABMAP_CORE_EXPORT Camera +class RTABMAP_CORE_EXPORT Camera : public SensorCapture { public: virtual ~Camera(); - SensorData takeImage(CameraInfo * info = 0); + + SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);} + float getImageRate() const {return getFrameRate();} + void setImageRate(float imageRate) {setFrameRate(imageRate);} + void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter + bool isInterIMUPublishing() const {return publishInterIMU_;} bool initFromFile(const std::string & calibrationPath); - virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0; virtual bool isCalibrated() const = 0; - virtual std::string getSerial() const = 0; - virtual bool odomProvided() const { return false; } - virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; } - - //getters - float getImageRate() const {return _imageRate;} - const Transform & getLocalTransform() const {return _localTransform;} - //setters - void setImageRate(float imageRate) {_imageRate = imageRate;} - void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;} - - void resetTimer(); protected: /** * Constructor @@ -78,19 +63,16 @@ class RTABMAP_CORE_EXPORT Camera */ Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity()); - /** - * returned rgb and depth images should be already rectified if calibration was loaded - */ - virtual SensorData captureImage(CameraInfo * info = 0) = 0; + virtual SensorData captureImage(SensorCaptureInfo * info = 0) = 0; - int getNextSeqID() {return ++_seq;} + void postInterIMU(const IMU & imu, double stamp); + +private: + virtual SensorData captureData(SensorCaptureInfo * info = 0) {return captureImage(info);} private: - float _imageRate; - Transform _localTransform; - cv::Size _targetImageSize; - UTimer * _frameRateTimer; - int _seq; + IMUFilter * imuFilter_; + bool publishInterIMU_; }; diff --git a/corelib/include/rtabmap/core/CameraEvent.h b/corelib/include/rtabmap/core/CameraEvent.h index cdb6c8eb84..5f58b73112 100644 --- a/corelib/include/rtabmap/core/CameraEvent.h +++ b/corelib/include/rtabmap/core/CameraEvent.h @@ -27,65 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include -#include "rtabmap/core/SensorData.h" -#include "rtabmap/core/CameraInfo.h" - -namespace rtabmap -{ - -class CameraEvent : - public UEvent -{ -public: - enum Code { - kCodeData, - kCodeNoMoreImages - }; - -public: - CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) : - UEvent(kCodeData), - data_(image, seq, stamp) - { - cameraInfo_.cameraName = cameraName; - } - - CameraEvent() : - UEvent(kCodeNoMoreImages) - { - } - - CameraEvent(const SensorData & data) : - UEvent(kCodeData), - data_(data) - { - } - - CameraEvent(const SensorData & data, const std::string & cameraName) : - UEvent(kCodeData), - data_(data) - { - cameraInfo_.cameraName = cameraName; - } - CameraEvent(const SensorData & data, const CameraInfo & cameraInfo) : - UEvent(kCodeData), - data_(data), - cameraInfo_(cameraInfo) - { - } - - // Image or descriptors - const SensorData & data() const {return data_;} - const std::string & cameraName() const {return cameraInfo_.cameraName;} - const CameraInfo & info() const {return cameraInfo_;} - - virtual ~CameraEvent() {} - virtual std::string getClassName() const {return std::string("CameraEvent");} - -private: - SensorData data_; - CameraInfo cameraInfo_; -}; - -} // namespace rtabmap +#include "rtabmap/core/SensorEvent.h" diff --git a/corelib/include/rtabmap/core/CameraInfo.h b/corelib/include/rtabmap/core/CameraInfo.h index 9243947152..409af719ae 100644 --- a/corelib/include/rtabmap/core/CameraInfo.h +++ b/corelib/include/rtabmap/core/CameraInfo.h @@ -27,50 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include - -namespace rtabmap -{ - -class CameraInfo -{ - -public: - CameraInfo() : - cameraName(""), - id(0), - stamp(0.0), - timeCapture(0.0f), - timeDisparity(0.0f), - timeMirroring(0.0f), - timeStereoExposureCompensation(0.0f), - timeImageDecimation(0.0f), - timeHistogramEqualization(0.0f), - timeScanFromDepth(0.0f), - timeUndistortDepth(0.0f), - timeBilateralFiltering(0.0f), - timeTotal(0.0f), - odomCovariance(cv::Mat::eye(6,6,CV_64FC1)) - { - } - virtual ~CameraInfo() {} - - std::string cameraName; - int id; - double stamp; - float timeCapture; - float timeDisparity; - float timeMirroring; - float timeStereoExposureCompensation; - float timeImageDecimation; - float timeHistogramEqualization; - float timeScanFromDepth; - float timeUndistortDepth; - float timeBilateralFiltering; - float timeTotal; - Transform odomPose; - cv::Mat odomCovariance; - std::vector odomVelocity; -}; - -} // namespace rtabmap +#include "rtabmap/core/SensorCaptureInfo.h" diff --git a/corelib/include/rtabmap/core/CameraThread.h b/corelib/include/rtabmap/core/CameraThread.h index b4535fa40f..0ca8c2ec6d 100644 --- a/corelib/include/rtabmap/core/CameraThread.h +++ b/corelib/include/rtabmap/core/CameraThread.h @@ -27,136 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines - -#include -#include -#include -#include - -namespace clams -{ -class DiscreteDepthDistortionModel; -} - -namespace rtabmap -{ - -class Camera; -class CameraInfo; -class SensorData; -class StereoDense; -class IMUFilter; -class Feature2D; - -/** - * Class CameraThread - * - */ -class RTABMAP_CORE_EXPORT CameraThread : - public UThread, - public UEventsSender -{ -public: - // ownership transferred - CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap()); - /** - * @param camera the camera to take images from - * @param odomSensor an odometry sensor to get a pose - * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame - */ - CameraThread(Camera * camera, - Camera * odomSensor, - const Transform & extrinsics, - double poseTimeOffset = 0.0, - float poseScaleFactor = 1.0f, - bool odomAsGt = false, - const ParametersMap & parameters = ParametersMap()); - CameraThread(Camera * camera, - bool odomAsGt, - const ParametersMap & parameters = ParametersMap()); - virtual ~CameraThread(); - - void setMirroringEnabled(bool enabled) {_mirroring = enabled;} - void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;} - void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} - void setImageDecimation(int decimation) {_imageDecimation = decimation;} - void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;} - void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;} - void setImageRate(float imageRate); - void setDistortionModel(const std::string & path); - void enableBilateralFiltering(float sigmaS, float sigmaR); - void disableBilateralFiltering() {_bilateralFiltering = false;} - void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false); - void disableIMUFiltering(); - void enableFeatureDetection(const ParametersMap & parameters = ParametersMap()); - void disableFeatureDetection(); - - // Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False. - RTABMAP_DEPRECATED void setScanParameters( - bool fromDepth, - int downsampleStep, // decimation of the depth image in case the scan is from depth image - float rangeMin, - float rangeMax, - float voxelSize, - int normalsK, - int normalsRadius, - bool forceGroundNormalsUp); - void setScanParameters( - bool fromDepth, - int downsampleStep=1, // decimation of the depth image in case the scan is from depth image - float rangeMin=0.0f, - float rangeMax=0.0f, - float voxelSize = 0.0f, - int normalsK = 0, - int normalsRadius = 0.0f, - float groundNormalsUp = 0.0f); - - void postUpdate(SensorData * data, CameraInfo * info = 0) const; - - //getters - bool isPaused() const {return !this->isRunning();} - bool isCapturing() const {return this->isRunning();} - bool odomProvided() const; - - Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted - Camera * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted - -private: - virtual void mainLoopBegin(); - virtual void mainLoop(); - virtual void mainLoopKill(); - -private: - Camera * _camera; - Camera * _odomSensor; - Transform _extrinsicsOdomToCamera; - bool _odomAsGt; - double _poseTimeOffset; - float _poseScaleFactor; - bool _mirroring; - bool _stereoExposureCompensation; - bool _colorOnly; - int _imageDecimation; - int _histogramMethod; - bool _stereoToDepth; - bool _scanFromDepth; - int _scanDownsampleStep; - float _scanRangeMin; - float _scanRangeMax; - float _scanVoxelSize; - int _scanNormalsK; - float _scanNormalsRadius; - float _scanForceGroundNormalsUp; - StereoDense * _stereoDense; - clams::DiscreteDepthDistortionModel * _distortionModel; - bool _bilateralFiltering; - float _bilateralSigmaS; - float _bilateralSigmaR; - IMUFilter * _imuFilter; - bool _imuBaseFrameConversion; - Feature2D * _featureDetector; - bool _depthAsMask; -}; - -} // namespace rtabmap +#include "rtabmap/core/SensorCaptureThread.h" diff --git a/corelib/include/rtabmap/core/DBReader.h b/corelib/include/rtabmap/core/DBReader.h index f6fc97c00d..5bdc0e2008 100644 --- a/corelib/include/rtabmap/core/DBReader.h +++ b/corelib/include/rtabmap/core/DBReader.h @@ -86,10 +86,10 @@ class RTABMAP_CORE_EXPORT DBReader : public Camera { const DBDriver * driver() const {return _dbDriver;} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: - SensorData getNextData(CameraInfo * info = 0); + SensorData getNextData(SensorCaptureInfo * info = 0); private: std::list _paths; diff --git a/corelib/include/rtabmap/core/IMUFilter.h b/corelib/include/rtabmap/core/IMUFilter.h index f93d3f9179..6bd97d4776 100644 --- a/corelib/include/rtabmap/core/IMUFilter.h +++ b/corelib/include/rtabmap/core/IMUFilter.h @@ -28,12 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ #define CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines #include #include namespace rtabmap { -class IMUFilter +class RTABMAP_CORE_EXPORT IMUFilter { public: enum Type { diff --git a/corelib/include/rtabmap/core/LaserScan.h b/corelib/include/rtabmap/core/LaserScan.h index 33999a7b59..0e76fdcb76 100644 --- a/corelib/include/rtabmap/core/LaserScan.h +++ b/corelib/include/rtabmap/core/LaserScan.h @@ -47,7 +47,8 @@ class RTABMAP_CORE_EXPORT LaserScan kXYZRGB=7, kXYZNormal=8, kXYZINormal=9, - kXYZRGBNormal=10}; + kXYZRGBNormal=10, + kXYZIT=11}; static std::string formatName(const Format & format); static int channels(const Format & format); @@ -55,6 +56,7 @@ class RTABMAP_CORE_EXPORT LaserScan static bool isScanHasNormals(const Format & format); static bool isScanHasRGB(const Format & format); static bool isScanHasIntensity(const Format & format); + static bool isScanHasTime(const Format & format); static LaserScan backwardCompatibility( const cv::Mat & oldScanFormat, int maxPoints = 0, @@ -121,22 +123,27 @@ class RTABMAP_CORE_EXPORT LaserScan float angleMin() const {return angleMin_;} float angleMax() const {return angleMax_;} float angleIncrement() const {return angleIncrement_;} + void setLocalTransform(const Transform & t) {localTransform_ = t;} Transform localTransform() const {return localTransform_;} bool empty() const {return data_.empty();} bool isEmpty() const {return data_.empty();} - int size() const {return data_.cols;} + int size() const {return data_.total();} int dataType() const {return data_.type();} bool is2d() const {return isScan2d(format_);} bool hasNormals() const {return isScanHasNormals(format_);} bool hasRGB() const {return isScanHasRGB(format_);} bool hasIntensity() const {return isScanHasIntensity(format_);} + bool hasTime() const {return isScanHasTime(format_);} bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;} + bool isOrganized() const {return data_.rows > 1;} LaserScan clone() const; + LaserScan densify() const; int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;} int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;} int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;} + int getTimeOffset() const {return hasTime()?4:-1;} float & field(unsigned int pointIndex, unsigned int channelOffset); diff --git a/corelib/include/rtabmap/core/Lidar.h b/corelib/include/rtabmap/core/Lidar.h new file mode 100644 index 0000000000..fd4b821556 --- /dev/null +++ b/corelib/include/rtabmap/core/Lidar.h @@ -0,0 +1,57 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#pragma once + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines +#include + +namespace rtabmap +{ + +/** + * Class Lidar + * + */ +class RTABMAP_CORE_EXPORT Lidar : public SensorCapture +{ +public: + virtual ~Lidar() {} + +protected: + /** + * Constructor + * + * @param lidarRate the frame rate (Hz), 0 for fast as the lidar can + * @param localTransform the transform from base frame to lidar frame + */ + Lidar(float lidarRate = 0, const Transform & localTransform = Transform::getIdentity()) : + SensorCapture(lidarRate, localTransform) {} +}; + + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index 258be65836..8b89767309 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -111,6 +111,7 @@ class RTABMAP_CORE_EXPORT Odometry bool _alignWithGround; bool _publishRAMUsage; bool _imagesAlreadyRectified; + bool _deskewing; Transform _pose; int _resetCurrentCount; double previousStamp_; diff --git a/corelib/include/rtabmap/core/OdometryInfo.h b/corelib/include/rtabmap/core/OdometryInfo.h index 451a2d9e72..19b3e86c06 100644 --- a/corelib/include/rtabmap/core/OdometryInfo.h +++ b/corelib/include/rtabmap/core/OdometryInfo.h @@ -50,6 +50,7 @@ class OdometryInfo localBundleConstraints(0), localBundleTime(0), keyFrameAdded(false), + timeDeskewing(0.0f), timeEstimation(0.0f), timeParticleFiltering(0.0f), stamp(0), @@ -76,6 +77,7 @@ class OdometryInfo output.localBundlePoses = localBundlePoses; output.localBundleModels = localBundleModels; output.keyFrameAdded = keyFrameAdded; + output.timeDeskewing = timeDeskewing; output.timeEstimation = timeEstimation; output.timeParticleFiltering = timeParticleFiltering; output.stamp = stamp; @@ -105,6 +107,7 @@ class OdometryInfo std::map localBundlePoses; std::map > localBundleModels; bool keyFrameAdded; + float timeDeskewing; float timeEstimation; float timeParticleFiltering; double stamp; diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 70319d2e88..4173a0570d 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -463,6 +463,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.9, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame."); RTABMAP_PARAM(Odom, ImageDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str())); RTABMAP_PARAM(Odom, AlignWithGround, bool, false, "Align odometry with the ground on initialization."); + RTABMAP_PARAM(Odom, Deskewing, bool, true, "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided)."); // Odometry Frame-to-Map RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words."); @@ -752,6 +753,7 @@ class RTABMAP_CORE_EXPORT Parameters RTABMAP_PARAM(Icp, Epsilon, float, 0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution."); RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.1, "Ratio of matching correspondences to accept the transform."); RTABMAP_PARAM(Icp, Force4DoF, bool, false, uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str())); + RTABMAP_PARAM(Icp, FiltersEnabled, int, 3, "Flag to enable filters: 1=\"from\" cloud only, 2=\"to\" cloud only, 3=both."); #ifdef RTABMAP_POINTMATCHER RTABMAP_PARAM(Icp, PointToPlane, bool, true, "Use point to plane ICP."); #else @@ -931,6 +933,7 @@ class RTABMAP_CORE_EXPORT Parameters static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false); static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false); + static void readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly = false); static void writeINI(const std::string & configFile, const ParametersMap & parameters); /** diff --git a/corelib/include/rtabmap/core/RegistrationIcp.h b/corelib/include/rtabmap/core/RegistrationIcp.h index d0d6ab972f..0735cfae1b 100644 --- a/corelib/include/rtabmap/core/RegistrationIcp.h +++ b/corelib/include/rtabmap/core/RegistrationIcp.h @@ -69,6 +69,7 @@ class RTABMAP_CORE_EXPORT RegistrationIcp : public Registration float _epsilon; float _correspondenceRatio; bool _force4DoF; + int _filtersEnabled; bool _pointToPlane; int _pointToPlaneK; float _pointToPlaneRadius; diff --git a/corelib/include/rtabmap/core/SensorCapture.h b/corelib/include/rtabmap/core/SensorCapture.h new file mode 100644 index 0000000000..20ee01b1a2 --- /dev/null +++ b/corelib/include/rtabmap/core/SensorCapture.h @@ -0,0 +1,93 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#pragma once + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include +#include +#include "rtabmap/core/SensorData.h" +#include +#include +#include +#include + +class UDirectory; +class UTimer; + +namespace rtabmap +{ + +/** + * Class Camera + * + */ +class RTABMAP_CORE_EXPORT SensorCapture +{ +public: + virtual ~SensorCapture(); + SensorData takeData(SensorCaptureInfo * info = 0); + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0; + virtual std::string getSerial() const = 0; + virtual bool odomProvided() const { return false; } + virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06) { return false; } + + //getters + float getFrameRate() const {return _frameRate;} + const Transform & getLocalTransform() const {return _localTransform;} + + //setters + void setFrameRate(float frameRate) {_frameRate = frameRate;} + void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;} + + void resetTimer(); +protected: + /** + * Constructor + * + * @param frameRate the frame rate (Hz), 0 for fast as the sensor can + * @param localTransform the transform from base frame to sensor frame + */ + SensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity()); + + /** + * returned rgb and depth images should be already rectified if calibration was loaded + */ + virtual SensorData captureData(SensorCaptureInfo * info = 0) = 0; + + int getNextSeqID() {return ++_seq;} + +private: + float _frameRate; + Transform _localTransform; + UTimer * _frameRateTimer; + int _seq; +}; + + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/SensorCaptureInfo.h b/corelib/include/rtabmap/core/SensorCaptureInfo.h new file mode 100644 index 0000000000..2b0b017ea5 --- /dev/null +++ b/corelib/include/rtabmap/core/SensorCaptureInfo.h @@ -0,0 +1,82 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#pragma once + +#include "rtabmap/core/Transform.h" +#include + +namespace rtabmap +{ + +class SensorCaptureInfo +{ + +public: + SensorCaptureInfo() : + cameraName(""), + id(0), + stamp(0.0), + timeCapture(0.0f), + timeDeskewing(0.0f), + timeDisparity(0.0f), + timeMirroring(0.0f), + timeStereoExposureCompensation(0.0f), + timeImageDecimation(0.0f), + timeHistogramEqualization(0.0f), + timeScanFromDepth(0.0f), + timeUndistortDepth(0.0f), + timeBilateralFiltering(0.0f), + timeTotal(0.0f), + odomCovariance(cv::Mat::eye(6,6,CV_64FC1)) + { + } + virtual ~SensorCaptureInfo() {} + + std::string cameraName; + int id; + double stamp; + float timeCapture; + float timeDeskewing; + float timeDisparity; + float timeMirroring; + float timeStereoExposureCompensation; + float timeImageDecimation; + float timeHistogramEqualization; + float timeScanFromDepth; + float timeUndistortDepth; + float timeBilateralFiltering; + float timeTotal; + Transform odomPose; + cv::Mat odomCovariance; + std::vector odomVelocity; +}; + +//backward compatibility +RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo; + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/SensorCaptureThread.h b/corelib/include/rtabmap/core/SensorCaptureThread.h new file mode 100644 index 0000000000..5013381f92 --- /dev/null +++ b/corelib/include/rtabmap/core/SensorCaptureThread.h @@ -0,0 +1,216 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#pragma once + +#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines + +#include +#include +#include +#include + +namespace clams +{ +class DiscreteDepthDistortionModel; +} + +namespace rtabmap +{ + +class Camera; +class Lidar; +class SensorCapture; +class SensorCaptureInfo; +class SensorData; +class StereoDense; +class IMUFilter; +class Feature2D; + +/** + * Class CameraThread + * + */ +class RTABMAP_CORE_EXPORT SensorCaptureThread : + public UThread, + public UEventsSender +{ +public: + // ownership transferred + SensorCaptureThread( + Camera * camera, + const ParametersMap & parameters = ParametersMap()); + /** + * @param camera the camera to take images from + * @param odomSensor an odometry sensor to get a pose (can be again the camera) + * @param odomAsGt set odometry sensor pose as ground truth instead of odometry + * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame (without optical rotation) + */ + SensorCaptureThread( + Camera * camera, + SensorCapture * odomSensor, + const Transform & extrinsics, + double poseTimeOffset = 0.0, + float poseScaleFactor = 1.0f, + double poseWaitTime = 0.1, + const ParametersMap & parameters = ParametersMap()); + /** + * @param lidar the lidar to take scans from + */ + SensorCaptureThread( + Lidar * lidar, + const ParametersMap & parameters = ParametersMap()); + /** + * @param lidar the lidar to take scans from + * @param camera the camera to take images from. If the camera is providing a pose, it can be used for deskewing + */ + SensorCaptureThread( + Lidar * lidar, + Camera * camera, + const ParametersMap & parameters = ParametersMap()); + /** + * @param lidar the lidar to take scans from + * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the lidar) + */ + SensorCaptureThread( + Lidar * lidar, + SensorCapture * odomSensor, + double poseTimeOffset = 0.0, + float poseScaleFactor = 1.0f, + double poseWaitTime = 0.1, + const ParametersMap & parameters = ParametersMap()); + /** + * @param lidar the lidar to take scans from + * @param camera the camera to take images from + * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the camera or lidar) + * @param extrinsics the static transform between odometry frame to camera frame (without optical rotation) + */ + SensorCaptureThread( + Lidar * lidar, + Camera * camera, + SensorCapture * odomSensor, + const Transform & extrinsics, + double poseTimeOffset = 0.0, + float poseScaleFactor = 1.0f, + double poseWaitTime = 0.1, + const ParametersMap & parameters = ParametersMap()); + virtual ~SensorCaptureThread(); + + void setMirroringEnabled(bool enabled) {_mirroring = enabled;} + void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;} + void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;} + void setImageDecimation(int decimation) {_imageDecimation = decimation;} + void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;} + void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;} + void setFrameRate(float frameRate); + RTABMAP_DEPRECATED void setImageRate(float frameRate) {setFrameRate(frameRate);} + void setDistortionModel(const std::string & path); + void setOdomAsGroundTruth(bool enabled) {_odomAsGt = enabled;} + void enableBilateralFiltering(float sigmaS, float sigmaR); + void disableBilateralFiltering() {_bilateralFiltering = false;} + void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false); + void disableIMUFiltering(); + void enableFeatureDetection(const ParametersMap & parameters = ParametersMap()); + void disableFeatureDetection(); + + // Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False. + RTABMAP_DEPRECATED void setScanParameters( + bool fromDepth, + int downsampleStep, // decimation of the depth image in case the scan is from depth image + float rangeMin, + float rangeMax, + float voxelSize, + int normalsK, + float normalsRadius, + bool forceGroundNormalsUp, + bool deskewing); + void setScanParameters( + bool fromDepth, + int downsampleStep=1, // decimation of the depth image in case the scan is from depth image + float rangeMin=0.0f, + float rangeMax=0.0f, + float voxelSize = 0.0f, + int normalsK = 0, + float normalsRadius = 0.0f, + float groundNormalsUp = 0.0f, + bool deskewing = false); + + void postUpdate(SensorData * data, SensorCaptureInfo * info = 0) const; + + //getters + bool isPaused() const {return !this->isRunning();} + bool isCapturing() const {return this->isRunning();} + bool odomProvided() const; + + Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted + SensorCapture * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted + Lidar * lidar() {return _lidar;} // return null if not set, valid until CameraThread is deleted + +private: + virtual void mainLoopBegin(); + virtual void mainLoop(); + virtual void mainLoopKill(); + +private: + Camera * _camera; + SensorCapture * _odomSensor; + Lidar * _lidar; + Transform _extrinsicsOdomToCamera; + bool _odomAsGt; + double _poseTimeOffset; + float _poseScaleFactor; + double _poseWaitTime; + bool _mirroring; + bool _stereoExposureCompensation; + bool _colorOnly; + int _imageDecimation; + int _histogramMethod; + bool _stereoToDepth; + bool _scanDeskewing; + bool _scanFromDepth; + int _scanDownsampleStep; + float _scanRangeMin; + float _scanRangeMax; + float _scanVoxelSize; + int _scanNormalsK; + float _scanNormalsRadius; + float _scanForceGroundNormalsUp; + StereoDense * _stereoDense; + clams::DiscreteDepthDistortionModel * _distortionModel; + bool _bilateralFiltering; + float _bilateralSigmaS; + float _bilateralSigmaR; + IMUFilter * _imuFilter; + bool _imuBaseFrameConversion; + Feature2D * _featureDetector; + bool _depthAsMask; +}; + +//backward compatibility +RTABMAP_DEPRECATED typedef SensorCaptureThread CameraThread; + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/SensorEvent.h b/corelib/include/rtabmap/core/SensorEvent.h new file mode 100644 index 0000000000..240f5f0010 --- /dev/null +++ b/corelib/include/rtabmap/core/SensorEvent.h @@ -0,0 +1,94 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#pragma once + +#include +#include +#include "rtabmap/core/SensorData.h" + +namespace rtabmap +{ + +class SensorEvent : + public UEvent +{ +public: + enum Code { + kCodeData, + kCodeNoMoreImages + }; + +public: + SensorEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) : + UEvent(kCodeData), + data_(image, seq, stamp) + { + sensorCaptureInfo_.cameraName = cameraName; + } + + SensorEvent() : + UEvent(kCodeNoMoreImages) + { + } + + SensorEvent(const SensorData & data) : + UEvent(kCodeData), + data_(data) + { + } + + SensorEvent(const SensorData & data, const std::string & cameraName) : + UEvent(kCodeData), + data_(data) + { + sensorCaptureInfo_.cameraName = cameraName; + } + SensorEvent(const SensorData & data, const SensorCaptureInfo & sensorCaptureInfo) : + UEvent(kCodeData), + data_(data), + sensorCaptureInfo_(sensorCaptureInfo) + { + } + + // Image or descriptors + const SensorData & data() const {return data_;} + const std::string & cameraName() const {return sensorCaptureInfo_.cameraName;} + const SensorCaptureInfo & info() const {return sensorCaptureInfo_;} + + virtual ~SensorEvent() {} + virtual std::string getClassName() const {return std::string("SensorEvent");} + +private: + SensorData data_; + SensorCaptureInfo sensorCaptureInfo_; +}; + +//backward compatibility +RTABMAP_DEPRECATED typedef SensorEvent CameraEvent; + +} // namespace rtabmap diff --git a/corelib/include/rtabmap/core/camera/CameraDepthAI.h b/corelib/include/rtabmap/core/camera/CameraDepthAI.h index a7bb4fa29e..fa359ad098 100644 --- a/corelib/include/rtabmap/core/camera/CameraDepthAI.h +++ b/corelib/include/rtabmap/core/camera/CameraDepthAI.h @@ -73,7 +73,7 @@ class RTABMAP_CORE_EXPORT CameraDepthAI : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_DEPTHAI diff --git a/corelib/include/rtabmap/core/camera/CameraFreenect.h b/corelib/include/rtabmap/core/camera/CameraFreenect.h index 3373e04e45..4c7c62ffe9 100644 --- a/corelib/include/rtabmap/core/camera/CameraFreenect.h +++ b/corelib/include/rtabmap/core/camera/CameraFreenect.h @@ -61,7 +61,7 @@ class RTABMAP_CORE_EXPORT CameraFreenect : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_FREENECT diff --git a/corelib/include/rtabmap/core/camera/CameraFreenect2.h b/corelib/include/rtabmap/core/camera/CameraFreenect2.h index 70210c0069..d0516074c5 100644 --- a/corelib/include/rtabmap/core/camera/CameraFreenect2.h +++ b/corelib/include/rtabmap/core/camera/CameraFreenect2.h @@ -77,7 +77,7 @@ class RTABMAP_CORE_EXPORT CameraFreenect2 : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_FREENECT2 diff --git a/corelib/include/rtabmap/core/camera/CameraImages.h b/corelib/include/rtabmap/core/camera/CameraImages.h index 0a38c52c2d..9b7134dc99 100644 --- a/corelib/include/rtabmap/core/camera/CameraImages.h +++ b/corelib/include/rtabmap/core/camera/CameraImages.h @@ -118,7 +118,7 @@ class RTABMAP_CORE_EXPORT CameraImages : } protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: bool readPoses( diff --git a/corelib/include/rtabmap/core/camera/CameraK4A.h b/corelib/include/rtabmap/core/camera/CameraK4A.h index b942ea2086..0629af4693 100644 --- a/corelib/include/rtabmap/core/camera/CameraK4A.h +++ b/corelib/include/rtabmap/core/camera/CameraK4A.h @@ -63,7 +63,7 @@ class RTABMAP_CORE_EXPORT CameraK4A : void setPreferences(int rgb_resolution, int framerate, int depth_resolution); protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: void close(); diff --git a/corelib/include/rtabmap/core/camera/CameraK4W2.h b/corelib/include/rtabmap/core/camera/CameraK4W2.h index 26581471ae..a47a6378a4 100644 --- a/corelib/include/rtabmap/core/camera/CameraK4W2.h +++ b/corelib/include/rtabmap/core/camera/CameraK4W2.h @@ -72,7 +72,7 @@ class RTABMAP_CORE_EXPORT CameraK4W2 : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: void close(); diff --git a/corelib/include/rtabmap/core/camera/CameraMyntEye.h b/corelib/include/rtabmap/core/camera/CameraMyntEye.h index 648d0846c8..d6037946a7 100644 --- a/corelib/include/rtabmap/core/camera/CameraMyntEye.h +++ b/corelib/include/rtabmap/core/camera/CameraMyntEye.h @@ -67,7 +67,7 @@ class RTABMAP_CORE_EXPORT CameraMyntEye : public Camera /** * returned rgb and depth images should be already rectified if calibration was loaded */ - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_MYNTEYE diff --git a/corelib/include/rtabmap/core/camera/CameraOpenNI2.h b/corelib/include/rtabmap/core/camera/CameraOpenNI2.h index 32d14a4625..dffbcfef98 100644 --- a/corelib/include/rtabmap/core/camera/CameraOpenNI2.h +++ b/corelib/include/rtabmap/core/camera/CameraOpenNI2.h @@ -69,7 +69,7 @@ class RTABMAP_CORE_EXPORT CameraOpenNI2 : void setDepthDecimation(int decimation); protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_OPENNI2 diff --git a/corelib/include/rtabmap/core/camera/CameraOpenNICV.h b/corelib/include/rtabmap/core/camera/CameraOpenNICV.h index 41a0669acf..9cb467d13c 100644 --- a/corelib/include/rtabmap/core/camera/CameraOpenNICV.h +++ b/corelib/include/rtabmap/core/camera/CameraOpenNICV.h @@ -51,7 +51,7 @@ class RTABMAP_CORE_EXPORT CameraOpenNICV : virtual std::string getSerial() const {return "";} // unknown with OpenCV protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: bool _asus; diff --git a/corelib/include/rtabmap/core/camera/CameraOpenni.h b/corelib/include/rtabmap/core/camera/CameraOpenni.h index 31af10515e..37a4425921 100644 --- a/corelib/include/rtabmap/core/camera/CameraOpenni.h +++ b/corelib/include/rtabmap/core/camera/CameraOpenni.h @@ -85,7 +85,7 @@ class RTABMAP_CORE_EXPORT CameraOpenni : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: pcl::Grabber* interface_; diff --git a/corelib/include/rtabmap/core/camera/CameraRGBDImages.h b/corelib/include/rtabmap/core/camera/CameraRGBDImages.h index 03189a09ff..ebc8870b14 100644 --- a/corelib/include/rtabmap/core/camera/CameraRGBDImages.h +++ b/corelib/include/rtabmap/core/camera/CameraRGBDImages.h @@ -53,7 +53,7 @@ class RTABMAP_CORE_EXPORT CameraRGBDImages : virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: CameraImages cameraDepth_; diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense.h b/corelib/include/rtabmap/core/camera/CameraRealSense.h index bf9b4de5c9..9bfd01c8d0 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense.h @@ -72,7 +72,7 @@ class RTABMAP_CORE_EXPORT CameraRealSense : virtual bool odomProvided() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_REALSENSE diff --git a/corelib/include/rtabmap/core/camera/CameraRealSense2.h b/corelib/include/rtabmap/core/camera/CameraRealSense2.h index e8e02e4847..47788f0f42 100644 --- a/corelib/include/rtabmap/core/camera/CameraRealSense2.h +++ b/corelib/include/rtabmap/core/camera/CameraRealSense2.h @@ -68,7 +68,7 @@ class RTABMAP_CORE_EXPORT CameraRealSense2 : virtual bool isCalibrated() const; virtual std::string getSerial() const; virtual bool odomProvided() const; - virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance); + virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // parameters are set during initialization // D400 series @@ -77,7 +77,7 @@ class RTABMAP_CORE_EXPORT CameraRealSense2 : void setResolution(int width, int height, int fps = 30); void setDepthResolution(int width, int height, int fps = 30); void setGlobalTimeSync(bool enabled); - void publishInterIMU(bool enabled); + /** * Dual mode (D400+T265 or L500+T265) * @param enabled enable dual mode @@ -105,7 +105,7 @@ class RTABMAP_CORE_EXPORT CameraRealSense2 : #endif protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_REALSENSE2 @@ -142,7 +142,6 @@ class RTABMAP_CORE_EXPORT CameraRealSense2 : int cameraDepthHeight_; int cameraDepthFps_; bool globalTimeSync_; - bool publishInterIMU_; bool dualMode_; Transform dualExtrinsics_; std::string jsonConfig_; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h b/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h index 13832204b5..025566b0e9 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h @@ -51,7 +51,7 @@ class RTABMAP_CORE_EXPORT CameraStereoDC1394 : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_DC1394 diff --git a/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h b/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h index 093f40865c..3b42788486 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h @@ -53,7 +53,7 @@ class RTABMAP_CORE_EXPORT CameraStereoFlyCapture2 : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_FLYCAPTURE2 diff --git a/corelib/include/rtabmap/core/camera/CameraStereoImages.h b/corelib/include/rtabmap/core/camera/CameraStereoImages.h index ed3a3ae303..75b43bbf68 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoImages.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoImages.h @@ -64,7 +64,7 @@ class RTABMAP_CORE_EXPORT CameraStereoImages : virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);camera2_->setMaxFrames(value);} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: CameraImages * camera2_; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoTara.h b/corelib/include/rtabmap/core/camera/CameraStereoTara.h index d6a655e4cf..72f5e12265 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoTara.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoTara.h @@ -60,7 +60,7 @@ public Camera virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: cv::VideoCapture capture_; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoVideo.h b/corelib/include/rtabmap/core/camera/CameraStereoVideo.h index 90042e05c3..b2e17d518d 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoVideo.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoVideo.h @@ -71,7 +71,7 @@ class RTABMAP_CORE_EXPORT CameraStereoVideo : void setResolution(int width, int height) {_width=width, _height=height;} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: cv::VideoCapture capture_; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZed.h b/corelib/include/rtabmap/core/camera/CameraStereoZed.h index 3e98d19981..640eec6c5a 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZed.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZed.h @@ -76,12 +76,12 @@ class RTABMAP_CORE_EXPORT CameraStereoZed : virtual bool isCalibrated() const; virtual std::string getSerial() const; virtual bool odomProvided() const; - virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance); + virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0); - void publishInterIMU(bool enabled); + void postInterIMUPublic(const IMU & imu, double stamp); protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_ZED @@ -100,7 +100,6 @@ class RTABMAP_CORE_EXPORT CameraStereoZed : bool computeOdometry_; bool lost_; bool force3DoF_; - bool publishInterIMU_; ZedIMUThread * imuPublishingThread_; #endif }; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h b/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h index 5f31ea960e..2399b7b174 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h @@ -63,7 +63,7 @@ class RTABMAP_CORE_EXPORT CameraStereoZedOC : virtual std::string getSerial() const; protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: #ifdef RTABMAP_ZEDOC diff --git a/corelib/include/rtabmap/core/camera/CameraVideo.h b/corelib/include/rtabmap/core/camera/CameraVideo.h index da7124937f..d34e0e9b45 100644 --- a/corelib/include/rtabmap/core/camera/CameraVideo.h +++ b/corelib/include/rtabmap/core/camera/CameraVideo.h @@ -64,7 +64,7 @@ class RTABMAP_CORE_EXPORT CameraVideo : void setResolution(int width, int height) {_width=width, _height=height;} protected: - virtual SensorData captureImage(CameraInfo * info = 0); + virtual SensorData captureImage(SensorCaptureInfo * info = 0); private: // File type diff --git a/corelib/include/rtabmap/core/lidar/LidarVLP16.h b/corelib/include/rtabmap/core/lidar/LidarVLP16.h new file mode 100644 index 0000000000..bb8df04bdb --- /dev/null +++ b/corelib/include/rtabmap/core/lidar/LidarVLP16.h @@ -0,0 +1,94 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_ +#define CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_ + +// Should be first on windows to avoid "WinSock.h has already been included" error +#include + +#include +#include + +namespace rtabmap { + +struct PointXYZIT { + float x; + float y; + float z; + float i; + float t; +}; + +class RTABMAP_CORE_EXPORT LidarVLP16 :public Lidar, public pcl::VLPGrabber { +public: + LidarVLP16( + const std::string& pcapFile, + bool organized = false, + bool stampLast = true, + float frameRate = 0.0f, + Transform localTransform = Transform::getIdentity()); + LidarVLP16( + const boost::asio::ip::address& ipAddress, + const std::uint16_t port = 2368, + bool organized = false, + bool useHostTime = true, + bool stampLast = true, + float frameRate = 0.0f, + Transform localTransform = Transform::getIdentity()); + virtual ~LidarVLP16(); + + SensorData takeScan(SensorCaptureInfo * info = 0) {return takeData(info);} + + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") override; + virtual std::string getSerial() const override {return getName();} + + void setOrganized(bool enable); + +private: + void buildTimings(bool dualMode); + virtual void toPointClouds (HDLDataPacket *dataPacket) override; + +protected: + virtual SensorData captureData(SensorCaptureInfo * info = 0) override; + +private: + // timing offset lookup table + std::vector< std::vector > timingOffsets_; + bool timingOffsetsDualMode_; + double startSweepTime_; + double startSweepTimeHost_; + bool organized_; + bool useHostTime_; + bool stampLast_; + SensorData lastScan_; + std::vector > accumulatedScans_; + USemaphore scanReady_; + UMutex lastScanMutex_; +}; + +} /* namespace rtabmap */ + +#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_ */ diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h index 5a2e218fb1..9cba8724da 100644 --- a/corelib/include/rtabmap/core/util3d.h +++ b/corelib/include/rtabmap/core/util3d.h @@ -455,6 +455,19 @@ RTABMAP_DEPRECATED pcl::PointCloud::Ptr RTABMAP_CORE_EXPORT loadC int downsampleStep = 1, float voxelSize = 0.0f); +/** + * @brief Lidar deskewing + * @param input lidar, format should have time channel + * @param input stamp of the lidar + * @param velocity in base frame + * @param velocity stamp at which it has been computed + * @return lidar deskewed + */ +LaserScan RTABMAP_CORE_EXPORT deskew( + const LaserScan & input, + double inputStamp, + const rtabmap::Transform & velocity); + } // namespace util3d } // namespace rtabmap diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 39a59ae0c8..fd66c948a3 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -13,8 +13,10 @@ SET(SRC_FILES Recovery.cpp + SensorCapture.cpp + SensorCaptureThread.cpp + Camera.cpp - CameraThread.cpp CameraModel.cpp camera/CameraFreenect.cpp @@ -39,6 +41,8 @@ SET(SRC_FILES camera/CameraMyntEye.cpp camera/CameraDepthAI.cpp + lidar/LidarVLP16.cpp + EpipolarGeometry.cpp VisualWord.cpp VWDictionary.cpp diff --git a/corelib/src/Camera.cpp b/corelib/src/Camera.cpp index ef90651ad6..b9503c85ee 100644 --- a/corelib/src/Camera.cpp +++ b/corelib/src/Camera.cpp @@ -26,42 +26,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "rtabmap/core/Camera.h" +#include "rtabmap/core/IMUFilter.h" -#include -#include #include -#include #include #include -#include - -#include - -#include -#include +#include namespace rtabmap { Camera::Camera(float imageRate, const Transform & localTransform) : - _imageRate(imageRate), - _localTransform(localTransform*CameraModel::opticalRotation()), - _targetImageSize(0,0), - _frameRateTimer(new UTimer()), - _seq(0) -{ -} + SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()), + imuFilter_(0), + publishInterIMU_(false) +{} Camera::~Camera() { - UDEBUG(""); - delete _frameRateTimer; - UDEBUG(""); -} - -void Camera::resetTimer() -{ - _frameRateTimer->start(); + delete imuFilter_; } bool Camera::initFromFile(const std::string & calibrationPath) @@ -69,54 +52,32 @@ bool Camera::initFromFile(const std::string & calibrationPath) return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()); } -SensorData Camera::takeImage(CameraInfo * info) +void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter) { - bool warnFrameRateTooHigh = false; - float actualFrameRate = 0; - float imageRate = _imageRate; - if(imageRate>0) - { - int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime()); - if(sleepTime > 2) - { - uSleep(sleepTime-2); - } - else if(sleepTime < 0) - { - warnFrameRateTooHigh = true; - actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime()); - } - - // Add precision at the cost of a small overhead - while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001) - { - // - } - - double slept = _frameRateTimer->getElapsedTime(); - _frameRateTimer->start(); - UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate)); - } + publishInterIMU_ = enabled; + delete imuFilter_; + imuFilter_ = filter; +} - UTimer timer; - SensorData data = this->captureImage(info); - double captureTime = timer.ticks(); - if(warnFrameRateTooHigh) - { - UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.", - imageRate, actualFrameRate, captureTime); - } - else - { - UDEBUG("Time capturing image = %fs", captureTime); - } - if(info) +void Camera::postInterIMU(const IMU & imu, double stamp) +{ + if(imuFilter_) { - info->id = data.id(); - info->stamp = data.stamp(); - info->timeCapture = captureTime; + imuFilter_->update( + imu.angularVelocity()[0], imu.angularVelocity()[1], imu.angularVelocity()[2], + imu.linearAcceleration()[0], imu.linearAcceleration()[1], imu.linearAcceleration()[2], + stamp); + cv::Vec4d q; + imuFilter_->getOrientation(q[0],q[1],q[2],q[3]); + UEventsManager::post(new IMUEvent(IMU( + q, cv::Mat(), + imu.angularVelocity(), imu.angularVelocityCovariance(), + imu.linearAcceleration(), imu.linearAccelerationCovariance(), + imu.localTransform()), + stamp)); + return; } - return data; + UEventsManager::post(new IMUEvent(imu, stamp)); } } // namespace rtabmap diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index 83b66c592a..6eb27f796b 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/core/DBReader.h" #include "rtabmap/core/DBDriver.h" @@ -34,7 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/OdometryEvent.h" #include "rtabmap/core/util3d.h" @@ -268,7 +268,7 @@ std::string DBReader::getSerial() const return "DBReader"; } -SensorData DBReader::captureImage(CameraInfo * info) +SensorData DBReader::captureImage(SensorCaptureInfo * info) { SensorData data = this->getNextData(info); if(data.id()>0 && _stopId>0 && data.id() > _stopId) @@ -370,7 +370,7 @@ SensorData DBReader::captureImage(CameraInfo * info) return data; } -SensorData DBReader::getNextData(CameraInfo * info) +SensorData DBReader::getNextData(SensorCaptureInfo * info) { SensorData data; if(_dbDriver) diff --git a/corelib/src/LaserScan.cpp b/corelib/src/LaserScan.cpp index eb7a2cb77f..7e875a9834 100644 --- a/corelib/src/LaserScan.cpp +++ b/corelib/src/LaserScan.cpp @@ -65,6 +65,9 @@ std::string LaserScan::formatName(const Format & format) case kXYZRGBNormal: name = "XYZRGBNormal"; break; + case kXYZIT: + name = "XYZIT"; + break; default: name = "Unknown"; break; @@ -88,6 +91,7 @@ int LaserScan::channels(const Format & format) channels = 4; break; case kXYNormal: + case kXYZIT: channels = 5; break; case kXYZNormal: @@ -119,7 +123,11 @@ bool LaserScan::isScanHasRGB(const Format & format) } bool LaserScan::isScanHasIntensity(const Format & format) { - return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal; + return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT; +} +bool LaserScan::isScanHasTime(const Format & format) +{ + return format==kXYZIT; } LaserScan LaserScan::backwardCompatibility( @@ -213,7 +221,14 @@ LaserScan::LaserScan( const LaserScan & scan, int maxPoints, float maxRange, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { UASSERT(scan.empty() || scan.format() != kUnknown); init(scan.data(), scan.format(), 0, maxRange, 0, 0, 0, maxPoints, localTransform); @@ -224,7 +239,14 @@ LaserScan::LaserScan( int maxPoints, float maxRange, Format format, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { init(scan.data(), format, 0, maxRange, 0, 0, 0, maxPoints, localTransform); } @@ -234,7 +256,14 @@ LaserScan::LaserScan( int maxPoints, float maxRange, Format format, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { init(data, format, 0, maxRange, 0, 0, 0, maxPoints, localTransform); } @@ -246,7 +275,14 @@ LaserScan::LaserScan( float angleMin, float angleMax, float angleIncrement, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { UASSERT(scan.empty() || scan.format() != kUnknown); init(scan.data(), scan.format(), minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform); @@ -260,7 +296,14 @@ LaserScan::LaserScan( float angleMin, float angleMax, float angleIncrement, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { init(scan.data(), format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform); } @@ -273,7 +316,14 @@ LaserScan::LaserScan( float angleMin, float angleMax, float angleIncrement, - const Transform & localTransform) + const Transform & localTransform) : + format_(kUnknown), + maxPoints_(0), + rangeMin_(0), + rangeMax_(0), + angleMin_(0), + angleMax_(0), + angleIncrement_(0) { init(data, format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform); } @@ -289,8 +339,7 @@ void LaserScan::init( int maxPoints, const Transform & localTransform) { - UASSERT(data.empty() || data.rows == 1); - UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7)); + UASSERT(data.empty() || (data.type() == CV_8UC1 && data.rows == 1) || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7)); UASSERT(!localTransform.isNull()); bool is2D = false; @@ -307,6 +356,10 @@ void LaserScan::init( // 3D scan UASSERT(rangeMax>=rangeMin); maxPoints_ = maxPoints; + if(maxPoints_ == 0 && data.rows>1) + { + maxPoints_ = data.rows * data.cols; + } } data_ = data; @@ -320,18 +373,18 @@ void LaserScan::init( if(!data.empty() && !isCompressed()) { - if(is2D && data_.cols > maxPoints_) + if(is2D && (int)data_.total() > maxPoints_) { - UWARN("The number of points (%d) in the scan is over the maximum " + UWARN("The number of points (%ld) in the scan is over the maximum " "points (%d) defined by angle settings (min=%f max=%f inc=%f). " "The scan info may be wrong!", - data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_); + data_.total(), maxPoints_, angleMin_, angleMax_, angleIncrement_); } - else if(!is2D && maxPoints_>0 && data_.cols > maxPoints_) + else if(!is2D && maxPoints_>0 && (int)data_.total() > maxPoints_) { - UDEBUG("The number of points (%d) in the scan is over the maximum " + UDEBUG("The number of points (%ld) in the scan is over the maximum " "points (%d) defined by max points setting.", - data_.cols, maxPoints_); + data_.total(), maxPoints_); } if(format == kUnknown) @@ -350,7 +403,7 @@ void LaserScan::init( UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); - UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); + UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str()); } @@ -366,11 +419,36 @@ LaserScan LaserScan::clone() const return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone()); } +LaserScan LaserScan::densify() const +{ + if(!isOrganized()) + { + return *this; + } + cv::Mat output(1, data_.total(), data_.type()); + int oi = 0; + for(int i=0; i(i, j); + float * outputPtr = output.ptr(0, oi); + if(! (std::isnan(ptr[0]) || std::isnan(ptr[1]) || (!is2d() && std::isnan(ptr[2])))) + { + memcpy(outputPtr, ptr, data_.elemSize()); + ++oi; + } + } + } + return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0,oi)), maxPoints_, rangeMax_, format_, localTransform_.clone()); +} + float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset) { - UASSERT(pointIndex < (unsigned int)data_.cols); + UASSERT(pointIndex < (unsigned int)data_.total()); UASSERT(channelOffset < (unsigned int)data_.channels()); - return data_.ptr(0, pointIndex)[channelOffset]; + unsigned int row = pointIndex / data_.cols; + return data_.ptr(row, pointIndex - row * data_.cols)[channelOffset]; } LaserScan & LaserScan::operator+=(const LaserScan & scan) @@ -381,7 +459,7 @@ LaserScan & LaserScan::operator+=(const LaserScan & scan) LaserScan LaserScan::operator+(const LaserScan & scan) { - UASSERT(this->empty() || scan.empty() || this->format() == scan.format()); + UASSERT(this->empty() || scan.empty() || (this->format() == scan.format() && !this->isOrganized() && !scan.isOrganized())); LaserScan dest; if(!scan.empty()) { diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 56a3d5d304..23eb7a8463 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -140,6 +140,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _alignWithGround(Parameters::defaultOdomAlignWithGround()), _publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()), _imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()), + _deskewing(Parameters::defaultOdomDeskewing()), _pose(Transform::getIdentity()), _resetCurrentCount(0), previousStamp_(0), @@ -169,6 +170,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround); Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage); Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified); + Parameters::parse(parameters, Parameters::kOdomDeskewing(), _deskewing); if(_imageDecimation == 0) { @@ -620,6 +622,75 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet } UTimer time; + + // Deskewing lidar + if( _deskewing && + !data.laserScanRaw().empty() && + data.laserScanRaw().hasTime() && + dt > 0 && + !guess.isNull()) + { + UDEBUG("Deskewing begin"); + // Recompute velocity + float vx,vy,vz, vroll,vpitch,vyaw; + guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); + + // transform to velocity + vx /= dt; + vy /= dt; + vz /= dt; + vroll /= dt; + vpitch /= dt; + vyaw /= dt; + + if(!imus_.empty()) + { + float scanTime = + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] - + data.laserScanRaw().data().ptr(0, 0)[data.laserScanRaw().getTimeOffset()]; + + // replace orientation velocity based on IMU (if available) + Transform imuFirstScan = Transform::getTransform(imus_, + data.stamp() + + data.laserScanRaw().data().ptr(0, 0)[data.laserScanRaw().getTimeOffset()]); + Transform imuLastScan = Transform::getTransform(imus_, + data.stamp() + + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]); + if(!imuFirstScan.isNull() && !imuLastScan.isNull()) + { + Transform orientation = imuFirstScan.inverse() * imuLastScan; + orientation.getEulerAngles(vroll, vpitch, vyaw); + if(_force3DoF) + { + vroll=0; + vpitch=0; + vyaw /= scanTime; + } + else + { + vroll /= scanTime; + vpitch /= scanTime; + vyaw /= scanTime; + } + } + } + + Transform velocity(vx,vy,vz,vroll,vpitch,vyaw); + LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity); + if(!scanDeskewed.isEmpty()) + { + data.setLaserScan(scanDeskewed); + } + info->timeDeskewing = time.ticks(); + UDEBUG("Deskewing end"); + } + if(data.laserScanRaw().isOrganized()) + { + // Laser scans should be dense passing this point + data.setLaserScan(data.laserScanRaw().densify()); + } + + Transform t; if(_imageDecimation > 1 && !data.imageRaw().empty()) { diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp index 1256118291..ccf064a979 100644 --- a/corelib/src/OdometryThread.cpp +++ b/corelib/src/OdometryThread.cpp @@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/core/OdometryThread.h" #include "rtabmap/core/Odometry.h" #include "rtabmap/core/odometry/OdometryMono.h" #include "rtabmap/core/OdometryInfo.h" -#include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/OdometryEvent.h" #include "rtabmap/utilite/ULogger.h" @@ -58,12 +58,12 @@ bool OdometryThread::handleEvent(UEvent * event) { if(this->isRunning()) { - if(event->getClassName().compare("CameraEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - CameraEvent * cameraEvent = (CameraEvent*)event; - if(cameraEvent->getCode() == CameraEvent::kCodeData) + SensorEvent * sensorEvent = (SensorEvent*)event; + if(sensorEvent->getCode() == SensorEvent::kCodeData) { - this->addData(cameraEvent->data()); + this->addData(sensorEvent->data()); } } else if(event->getClassName().compare("IMUEvent") == 0) diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp index 0bd46c8f62..8c3dabdc72 100644 --- a/corelib/src/Parameters.cpp +++ b/corelib/src/Parameters.cpp @@ -1172,11 +1172,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam return out; } - -void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly) +void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, ParametersMap & parameters, bool modifiedOnly) { - CSimpleIniA ini; - ini.LoadFile(configFile.c_str()); const CSimpleIniA::TKeyVal * keyValMap = ini.GetSection("Core"); if(keyValMap) { @@ -1191,12 +1188,12 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet { if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str()))) { - if(configFile.find(".rtabmap") != std::string::npos) + if(configFilePath.find(".rtabmap") != std::string::npos) { UWARN("Version in the config file \"%s\" is more recent (\"%s\") than " "current RTAB-Map version used (\"%s\"). The config file will be upgraded " "to new version.", - configFile.c_str(), + configFilePath.c_str(), (*iter).second, RTABMAP_VERSION); } @@ -1205,7 +1202,7 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet UERROR("Version in the config file \"%s\" is more recent (\"%s\") than " "current RTAB-Map version used (\"%s\"). New parameters (if there are some) will " "be ignored.", - configFile.c_str(), + configFilePath.c_str(), (*iter).second, RTABMAP_VERSION); } @@ -1255,11 +1252,26 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet else { ULOGGER_WARN("Section \"Core\" in %s doesn't exist... " - "Ignore this warning if the ini file does not exist yet. " - "The ini file will be automatically created when rtabmap will close.", configFile.c_str()); + "Ignore this warning if the ini file does not exist yet. " + "The ini file will be automatically created when rtabmap will close.", configFilePath.c_str()); } } + +void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly) +{ + CSimpleIniA ini; + ini.LoadFile(configFile.c_str()); + readINIImpl(ini, configFile, parameters, modifiedOnly); +} + +void Parameters::readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly) +{ + CSimpleIniA ini; + ini.LoadData(configContent); + readINIImpl(ini, "", parameters, modifiedOnly); +} + void Parameters::writeINI(const std::string & configFile, const ParametersMap & parameters) { CSimpleIniA ini; diff --git a/corelib/src/Recovery.cpp b/corelib/src/Recovery.cpp index 5ad0a5e9d5..72546b031f 100644 --- a/corelib/src/Recovery.cpp +++ b/corelib/src/Recovery.cpp @@ -160,7 +160,7 @@ bool databaseRecovery( DBReader dbReader(databasePath, 0, odometryIgnored); dbReader.init(); - CameraInfo info; + SensorCaptureInfo info; SensorData data = dbReader.takeImage(&info); int processed = 0; if (progressState) diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 2327ae1a1e..11611ae11d 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -69,6 +69,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration _epsilon(Parameters::defaultIcpEpsilon()), _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()), _force4DoF(Parameters::defaultIcpForce4DoF()), + _filtersEnabled(Parameters::defaultIcpFiltersEnabled()), _pointToPlane(Parameters::defaultIcpPointToPlane()), _pointToPlaneK(Parameters::defaultIcpPointToPlaneK()), _pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()), @@ -115,6 +116,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon); Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio); Parameters::parse(parameters, Parameters::kIcpForce4DoF(), _force4DoF); + Parameters::parse(parameters, Parameters::kIcpFiltersEnabled(), _filtersEnabled); Parameters::parse(parameters, Parameters::kIcpOutlierRatio(), _outlierRatio); Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane); Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK); @@ -337,6 +339,7 @@ Transform RegistrationIcp::computeTransformationImpl( UDEBUG("Downsampling step=%d", _downsamplingStep); UDEBUG("Force 3DoF=%s", this->force3DoF()?"true":"false"); UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false"); + UDEBUG("Enabled filters: from=%s to=%s", _filtersEnabled&1?"true":"false", _filtersEnabled&2?"true":"false"); UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity); UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio); UDEBUG("Strategy=%d", _strategy); @@ -360,7 +363,7 @@ Transform RegistrationIcp::computeTransformationImpl( int maxLaserScansFrom = dataFrom.laserScanRaw().maxPoints()>0?dataFrom.laserScanRaw().maxPoints():dataFrom.laserScanRaw().size(); int maxLaserScansTo = dataTo.laserScanRaw().maxPoints()>0?dataTo.laserScanRaw().maxPoints():dataTo.laserScanRaw().size(); - if(!dataFrom.laserScanRaw().empty()) + if(!dataFrom.laserScanRaw().empty() && (_filtersEnabled & 1)) { int pointsBeforeFiltering = dataFrom.laserScanRaw().size(); LaserScan fromScan = util3d::commonFiltering(dataFrom.laserScanRaw(), @@ -401,7 +404,7 @@ Transform RegistrationIcp::computeTransformationImpl( float ratio = float(dataFrom.laserScanRaw().size()) / float(pointsBeforeFiltering); maxLaserScansFrom = int(float(maxLaserScansFrom) * ratio); } - if(!dataTo.laserScanRaw().empty()) + if(!dataTo.laserScanRaw().empty() && (_filtersEnabled & 2)) { int pointsBeforeFiltering = dataTo.laserScanRaw().size(); LaserScan toScan = util3d::commonFiltering(dataTo.laserScanRaw(), diff --git a/corelib/src/RtabmapThread.cpp b/corelib/src/RtabmapThread.cpp index 13b0c4fdda..d9f227b59f 100644 --- a/corelib/src/RtabmapThread.cpp +++ b/corelib/src/RtabmapThread.cpp @@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/RtabmapThread.h" #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/Camera.h" -#include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/ParamEvent.h" #include "rtabmap/core/OdometryEvent.h" #include "rtabmap/core/UserDataEvent.h" @@ -371,11 +371,11 @@ bool RtabmapThread::handleEvent(UEvent* event) // IMU events are published at high frequency, early exit return false; } - else if(event->getClassName().compare("CameraEvent") == 0) + else if(event->getClassName().compare("SensorEvent") == 0) { - UDEBUG("CameraEvent"); - CameraEvent * e = (CameraEvent*)event; - if(e->getCode() == CameraEvent::kCodeData) + UDEBUG("SensorEvent"); + SensorEvent * e = (SensorEvent*)event; + if(e->getCode() == SensorEvent::kCodeData) { if (_rtabmap->isRGBDMode()) { diff --git a/corelib/src/SensorCapture.cpp b/corelib/src/SensorCapture.cpp new file mode 100644 index 0000000000..59327878c2 --- /dev/null +++ b/corelib/src/SensorCapture.cpp @@ -0,0 +1,114 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include "rtabmap/core/SensorCapture.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace rtabmap +{ + +SensorCapture::SensorCapture(float frameRate, const Transform & localTransform) : + _frameRate(frameRate), + _localTransform(localTransform), + _frameRateTimer(new UTimer()), + _seq(0) +{ +} + +SensorCapture::~SensorCapture() +{ + delete _frameRateTimer; +} + +void SensorCapture::resetTimer() +{ + _frameRateTimer->start(); +} + +SensorData SensorCapture::takeData(SensorCaptureInfo * info) +{ + bool warnFrameRateTooHigh = false; + float actualFrameRate = 0; + float frameRate = _frameRate; + if(frameRate>0) + { + int sleepTime = (1000.0f/frameRate - 1000.0f*_frameRateTimer->getElapsedTime()); + if(sleepTime > 2) + { + uSleep(sleepTime-2); + } + else if(sleepTime < 0) + { + warnFrameRateTooHigh = true; + actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime()); + } + + // Add precision at the cost of a small overhead + while(_frameRateTimer->getElapsedTime() < 1.0/double(frameRate)-0.000001) + { + // + } + + double slept = _frameRateTimer->getElapsedTime(); + _frameRateTimer->start(); + UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate)); + } + + UTimer timer; + SensorData data = this->captureData(info); + double captureTime = timer.ticks(); + if(warnFrameRateTooHigh) + { + UWARN("Camera: Cannot reach target frame rate %f Hz, current rate is %f Hz and capture time = %f s.", + frameRate, actualFrameRate, captureTime); + } + else + { + UDEBUG("Time capturing data = %fs", captureTime); + } + if(info) + { + info->id = data.id(); + info->stamp = data.stamp(); + info->timeCapture = captureTime; + } + return data; +} + +} // namespace rtabmap diff --git a/corelib/src/CameraThread.cpp b/corelib/src/SensorCaptureThread.cpp similarity index 66% rename from corelib/src/CameraThread.cpp rename to corelib/src/SensorCaptureThread.cpp index 46997ad2b3..bac9c3aa5f 100644 --- a/corelib/src/CameraThread.cpp +++ b/corelib/src/SensorCaptureThread.cpp @@ -25,9 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include "rtabmap/core/CameraThread.h" +#include "rtabmap/core/SensorCaptureThread.h" #include "rtabmap/core/Camera.h" -#include "rtabmap/core/CameraEvent.h" +#include "rtabmap/core/Lidar.h" +#include "rtabmap/core/SensorEvent.h" #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/util2d.h" #include "rtabmap/core/util3d.h" @@ -50,143 +51,149 @@ namespace rtabmap { // ownership transferred -CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) : - _camera(camera), - _odomSensor(0), - _odomAsGt(false), - _poseTimeOffset(0.0), - _poseScaleFactor(1.0f), - _mirroring(false), - _stereoExposureCompensation(false), - _colorOnly(false), - _imageDecimation(1), - _histogramMethod(0), - _stereoToDepth(false), - _scanFromDepth(false), - _scanDownsampleStep(1), - _scanRangeMin(0.0f), - _scanRangeMax(0.0f), - _scanVoxelSize(0.0f), - _scanNormalsK(0), - _scanNormalsRadius(0.0f), - _scanForceGroundNormalsUp(false), - _stereoDense(StereoDense::create(parameters)), - _distortionModel(0), - _bilateralFiltering(false), - _bilateralSigmaS(10), - _bilateralSigmaR(0.1), - _imuFilter(0), - _imuBaseFrameConversion(false), - _featureDetector(0), - _depthAsMask(Parameters::defaultVisDepthAsMask()) +SensorCaptureThread::SensorCaptureThread( + Camera * camera, + const ParametersMap & parameters) : + SensorCaptureThread(0, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters) { - UASSERT(_camera != 0); + UASSERT(camera != 0); } // ownership transferred -CameraThread::CameraThread( +SensorCaptureThread::SensorCaptureThread( Camera * camera, - Camera * odomSensor, + SensorCapture * odomSensor, const Transform & extrinsics, double poseTimeOffset, float poseScaleFactor, - bool odomAsGt, + double poseWaitTime, const ParametersMap & parameters) : - _camera(camera), - _odomSensor(odomSensor), - _extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()), - _odomAsGt(odomAsGt), - _poseTimeOffset(poseTimeOffset), - _poseScaleFactor(poseScaleFactor), - _mirroring(false), - _stereoExposureCompensation(false), - _colorOnly(false), - _imageDecimation(1), - _histogramMethod(0), - _stereoToDepth(false), - _scanFromDepth(false), - _scanDownsampleStep(1), - _scanRangeMin(0.0f), - _scanRangeMax(0.0f), - _scanVoxelSize(0.0f), - _scanNormalsK(0), - _scanNormalsRadius(0.0f), - _scanForceGroundNormalsUp(false), - _stereoDense(StereoDense::create(parameters)), - _distortionModel(0), - _bilateralFiltering(false), - _bilateralSigmaS(10), - _bilateralSigmaR(0.1), - _imuFilter(0), - _imuBaseFrameConversion(false), - _featureDetector(0), - _depthAsMask(Parameters::defaultVisDepthAsMask()) + SensorCaptureThread(0, camera, odomSensor, extrinsics, poseTimeOffset, poseScaleFactor, poseWaitTime, parameters) { - UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull()); - UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str()); - UDEBUG("_poseTimeOffset =%f", _poseTimeOffset); - UDEBUG("_poseScaleFactor =%f", _poseScaleFactor); - UDEBUG("_odomAsGt =%s", _odomAsGt?"true":"false"); + UASSERT(camera != 0 && odomSensor != 0 && !extrinsics.isNull()); } -// ownership transferred -CameraThread::CameraThread( +SensorCaptureThread::SensorCaptureThread( + Lidar * lidar, + const ParametersMap & parameters) : + SensorCaptureThread(lidar, 0, 0, Transform(), 0.0, 1.0f, 0.1, parameters) +{ + UASSERT(lidar != 0); +} + +SensorCaptureThread::SensorCaptureThread( + Lidar * lidar, Camera * camera, - bool odomAsGt, const ParametersMap & parameters) : - _camera(camera), - _odomSensor(0), - _odomAsGt(odomAsGt), - _poseTimeOffset(0.0), - _poseScaleFactor(1.0f), - _mirroring(false), - _stereoExposureCompensation(false), - _colorOnly(false), - _imageDecimation(1), - _histogramMethod(0), - _stereoToDepth(false), - _scanFromDepth(false), - _scanDownsampleStep(1), - _scanRangeMin(0.0f), - _scanRangeMax(0.0f), - _scanVoxelSize(0.0f), - _scanNormalsK(0), - _scanNormalsRadius(0.0f), - _scanForceGroundNormalsUp(false), - _stereoDense(StereoDense::create(parameters)), - _distortionModel(0), - _bilateralFiltering(false), - _bilateralSigmaS(10), - _bilateralSigmaR(0.1), - _imuFilter(0), - _imuBaseFrameConversion(false), - _featureDetector(0), - _depthAsMask(Parameters::defaultVisDepthAsMask()) + SensorCaptureThread(lidar, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters) { - UASSERT(_camera != 0); - UDEBUG("_odomAsGt =%s", _odomAsGt?"true":"false"); + UASSERT(lidar != 0 && camera != 0); } -CameraThread::~CameraThread() +SensorCaptureThread::SensorCaptureThread( + Lidar * lidar, + SensorCapture * odomSensor, + double poseTimeOffset, + float poseScaleFactor, + double poseWaitTime, + const ParametersMap & parameters) : + SensorCaptureThread(lidar, 0, odomSensor, Transform(), poseTimeOffset, poseScaleFactor, poseWaitTime, parameters) +{ + UASSERT(lidar != 0 && odomSensor != 0); +} + +SensorCaptureThread::SensorCaptureThread( + Lidar * lidar, + Camera * camera, + SensorCapture * odomSensor, + const Transform & extrinsics, + double poseTimeOffset, + float poseScaleFactor, + double poseWaitTime, + const ParametersMap & parameters) : + _camera(camera), + _odomSensor(odomSensor), + _lidar(lidar), + _extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()), + _odomAsGt(false), + _poseTimeOffset(poseTimeOffset), + _poseScaleFactor(poseScaleFactor), + _poseWaitTime(poseWaitTime), + _mirroring(false), + _stereoExposureCompensation(false), + _colorOnly(false), + _imageDecimation(1), + _histogramMethod(0), + _stereoToDepth(false), + _scanDeskewing(false), + _scanFromDepth(false), + _scanDownsampleStep(1), + _scanRangeMin(0.0f), + _scanRangeMax(0.0f), + _scanVoxelSize(0.0f), + _scanNormalsK(0), + _scanNormalsRadius(0.0f), + _scanForceGroundNormalsUp(false), + _stereoDense(StereoDense::create(parameters)), + _distortionModel(0), + _bilateralFiltering(false), + _bilateralSigmaS(10), + _bilateralSigmaR(0.1), + _imuFilter(0), + _imuBaseFrameConversion(false), + _featureDetector(0), + _depthAsMask(Parameters::defaultVisDepthAsMask()) +{ + UASSERT(_camera != 0 || _lidar != 0); + if(_lidar && _camera) + { + _camera->setFrameRate(0); + } + if(_odomSensor) + { + if(_camera) + { + if(_odomSensor == _camera && _extrinsicsOdomToCamera.isNull()) + { + _extrinsicsOdomToCamera.setIdentity(); + } + UASSERT(!_extrinsicsOdomToCamera.isNull()); + UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str()); + } + UDEBUG("_poseTimeOffset =%f", _poseTimeOffset); + UDEBUG("_poseScaleFactor =%f", _poseScaleFactor); + UDEBUG("_poseWaitTime =%f", _poseWaitTime); + } +} + +SensorCaptureThread::~SensorCaptureThread() { join(true); + if(_odomSensor != _camera && _odomSensor != _lidar) + { + delete _odomSensor; + } delete _camera; - delete _odomSensor; + delete _lidar; delete _distortionModel; delete _stereoDense; delete _imuFilter; delete _featureDetector; } -void CameraThread::setImageRate(float imageRate) +void SensorCaptureThread::setFrameRate(float frameRate) { - if(_camera) + if(_lidar) { - _camera->setImageRate(imageRate); + _lidar->setFrameRate(frameRate); + } + else if(_camera) + { + _camera->setFrameRate(frameRate); } } -void CameraThread::setDistortionModel(const std::string & path) +void SensorCaptureThread::setDistortionModel(const std::string & path) { if(_distortionModel) { @@ -206,7 +213,7 @@ void CameraThread::setDistortionModel(const std::string & path) } } -void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR) +void SensorCaptureThread::enableBilateralFiltering(float sigmaS, float sigmaR) { UASSERT(sigmaS > 0.0f && sigmaR > 0.0f); _bilateralFiltering = true; @@ -214,20 +221,20 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR) _bilateralSigmaR = sigmaR; } -void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion) +void SensorCaptureThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion) { delete _imuFilter; _imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters); _imuBaseFrameConversion = baseFrameConversion; } -void CameraThread::disableIMUFiltering() +void SensorCaptureThread::disableIMUFiltering() { delete _imuFilter; _imuFilter = 0; } -void CameraThread::enableFeatureDetection(const ParametersMap & parameters) +void SensorCaptureThread::enableFeatureDetection(const ParametersMap & parameters) { delete _featureDetector; ParametersMap params = parameters; @@ -245,35 +252,38 @@ void CameraThread::enableFeatureDetection(const ParametersMap & parameters) _featureDetector = Feature2D::create(params); _depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask); } -void CameraThread::disableFeatureDetection() +void SensorCaptureThread::disableFeatureDetection() { delete _featureDetector; _featureDetector = 0; } -void CameraThread::setScanParameters( +void SensorCaptureThread::setScanParameters( bool fromDepth, int downsampleStep, float rangeMin, float rangeMax, float voxelSize, int normalsK, - int normalsRadius, - bool forceGroundNormalsUp) + float normalsRadius, + bool forceGroundNormalsUp, + bool deskewing) { - setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f); + setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f, deskewing); } -void CameraThread::setScanParameters( +void SensorCaptureThread::setScanParameters( bool fromDepth, int downsampleStep, // decimation of the depth image in case the scan is from depth image float rangeMin, float rangeMax, float voxelSize, int normalsK, - int normalsRadius, - float groundNormalsUp) + float normalsRadius, + float groundNormalsUp, + bool deskewing) { + _scanDeskewing = deskewing; _scanFromDepth = fromDepth; _scanDownsampleStep=downsampleStep; _scanRangeMin = rangeMin; @@ -284,34 +294,178 @@ void CameraThread::setScanParameters( _scanForceGroundNormalsUp = groundNormalsUp; } -bool CameraThread::odomProvided() const +bool SensorCaptureThread::odomProvided() const { - return _camera && (_camera->odomProvided() || (_odomSensor && _odomSensor->odomProvided())); + if(_odomAsGt) + { + return false; + } + return _odomSensor != 0; } -void CameraThread::mainLoopBegin() +void SensorCaptureThread::mainLoopBegin() { ULogger::registerCurrentThread("Camera"); + if(_lidar) + { + _lidar->resetTimer(); + } + else if(_camera) + { + _camera->resetTimer(); + } if(_imuFilter) { // In case we paused the camera and moved somewhere else, restart filtering. _imuFilter->reset(); } - _camera->resetTimer(); } -void CameraThread::mainLoop() +void SensorCaptureThread::mainLoop() { + UASSERT(_lidar || _camera); UTimer totalTime; - CameraInfo info; - SensorData data = _camera->takeImage(&info); + SensorCaptureInfo info; + SensorData data; + SensorData cameraData; + double lidarStamp = 0.0; + double cameraStamp = 0.0; + if(_lidar) + { + data = _lidar->takeData(&info); + if(data.stamp() == 0.0) + { + UERROR("Could not capture scan! Skipping this frame!"); + return; + } + else + { + lidarStamp = data.stamp(); + if(_camera) + { + cameraData = _camera->takeData(); + if(cameraData.stamp() == 0.0) + { + UERROR("Could not capture image! Skipping this frame!"); + return; + } + else + { + double stampStart = UTimer::now(); + while(cameraData.stamp() < data.stamp() && + !isKilled() && + UTimer::now() - stampStart < _poseWaitTime && + !cameraData.imageRaw().empty()) + { + // Make sure the camera frame is newer than lidar frame so + // that if there are imus published by the cameras, we can get + // them all in odometry before deskewing. + cameraData = _camera->takeData(); + } - if(_odomSensor) + cameraStamp = cameraData.stamp(); + if(cameraData.stamp() < data.stamp()) + { + UWARN("Could not get camera frame (%f) with stamp more recent than lidar frame (%f) after waiting for %f seconds.", + cameraData.stamp(), + data.stamp(), + _poseWaitTime); + } + + if(!cameraData.stereoCameraModels().empty()) + { + data.setStereoImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.stereoCameraModels(), true); + } + else + { + data.setRGBDImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.cameraModels(), true); + } + } + } + } + } + else if(_camera) { + data = _camera->takeData(&info); + if(data.stamp() == 0.0) + { + UERROR("Could not capture image! Skipping this frame!"); + return; + } + else + { + cameraStamp = cameraData.stamp(); + } + } + + if(_odomSensor && data.stamp() != 0.0) + { + if(lidarStamp!=0.0 && _scanDeskewing) + { + UDEBUG("Deskewing begin"); + if(!data.laserScanRaw().empty() && data.laserScanRaw().hasTime()) + { + float scanTime = + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] - + data.laserScanRaw().data().ptr(0, 0)[data.laserScanRaw().getTimeOffset()]; + + Transform poseFirstScan; + Transform poseLastScan; + cv::Mat cov; + double firstStamp = data.stamp() + data.laserScanRaw().data().ptr(0, 0)[data.laserScanRaw().getTimeOffset()]; + double lastStamp = data.stamp() + data.laserScanRaw().data().ptr(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]; + if(_odomSensor->getPose(firstStamp+_poseTimeOffset, poseFirstScan, cov, _poseWaitTime>0?_poseWaitTime:0) && + _odomSensor->getPose(lastStamp+_poseTimeOffset, poseLastScan, cov, _poseWaitTime>0?_poseWaitTime:0)) + { + if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f) + { + poseFirstScan.x() *= _poseScaleFactor; + poseFirstScan.y() *= _poseScaleFactor; + poseFirstScan.z() *= _poseScaleFactor; + poseLastScan.x() *= _poseScaleFactor; + poseLastScan.y() *= _poseScaleFactor; + poseLastScan.z() *= _poseScaleFactor; + } + + UASSERT(!poseFirstScan.isNull() && !poseLastScan.isNull()); + + Transform transform = poseFirstScan.inverse() * poseLastScan; + + // convert to velocity + float x,y,z,roll,pitch,yaw; + transform.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); + x/=scanTime; + y/=scanTime; + z/=scanTime; + roll /= scanTime; + pitch /= scanTime; + yaw /= scanTime; + + Transform velocity(x,y,z,roll,pitch,yaw); + UTimer timeDeskewing; + LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity); + info.timeDeskewing = timeDeskewing.ticks(); + if(!scanDeskewed.isEmpty()) + { + data.setLaserScan(scanDeskewed); + } + } + else + { + UWARN("Failed to get poses for stamps %f and %f! Skipping this frame!", firstStamp+_poseTimeOffset, lastStamp+_poseTimeOffset); + return; + } + } + else if(!data.laserScanRaw().empty()) + { + UWARN("The input scan doesn't have time channel (scan format received=%s)!. Lidar won't be deskewed!", data.laserScanRaw().formatName().c_str()); + } + UDEBUG("Deskewing end"); + } + Transform pose; - Transform poseToLeftCam; cv::Mat covariance; - if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance)) + if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0)) { info.odomPose = pose; info.odomCovariance = covariance; @@ -321,21 +475,47 @@ void CameraThread::mainLoop() info.odomPose.y() *= _poseScaleFactor; info.odomPose.z() *= _poseScaleFactor; } - // Adjust local transform of the camera based on the pose frame - if(!data.cameraModels().empty()) - { - UASSERT(data.cameraModels().size()==1); - CameraModel model = data.cameraModels()[0]; - model.setLocalTransform(_extrinsicsOdomToCamera); - data.setCameraModel(model); - } - else if(!data.stereoCameraModels().empty()) + + if(cameraStamp != 0.0) { - UASSERT(data.stereoCameraModels().size()==1); - StereoCameraModel model = data.stereoCameraModels()[0]; - model.setLocalTransform(_extrinsicsOdomToCamera); - data.setStereoCameraModel(model); + Transform cameraCorrection = Transform::getIdentity(); + if(lidarStamp > 0.0 && lidarStamp != cameraStamp) + { + if(_odomSensor->getPose(cameraStamp+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0)) + { + cameraCorrection = info.odomPose.inverse() * pose; + } + else + { + UWARN("Could not get pose at stamp %f, the camera local motion against lidar won't be adjusted.", cameraStamp); + } + } + + // Adjust local transform of the camera based on the pose frame + if(!data.cameraModels().empty()) + { + UASSERT(data.cameraModels().size()==1); + CameraModel model = data.cameraModels()[0]; + model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera); + data.setCameraModel(model); + } + else if(!data.stereoCameraModels().empty()) + { + UASSERT(data.stereoCameraModels().size()==1); + StereoCameraModel model = data.stereoCameraModels()[0]; + model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera); + data.setStereoCameraModel(model); + } } + + // Fake IMU to intialize gravity (assuming pose is aligned with gravity!) + Eigen::Quaterniond q = info.odomPose.getQuaterniond(); + data.setIMU(IMU( + cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(), + cv::Vec3d(), cv::Mat(), + cv::Vec3d(), cv::Mat(), + Transform::getIdentity())); + this->disableIMUFiltering(); } else { @@ -352,19 +532,19 @@ void CameraThread::mainLoop() if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set { postUpdate(&data, &info); - info.cameraName = _camera->getSerial(); + info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial(); info.timeTotal = totalTime.ticks(); - this->post(new CameraEvent(data, info)); + this->post(new SensorEvent(data, info)); } else if(!this->isKilled()) { - UWARN("no more images..."); + UWARN("no more data..."); this->kill(); - this->post(new CameraEvent()); + this->post(new SensorEvent()); } } -void CameraThread::mainLoopKill() +void SensorCaptureThread::mainLoopKill() { if(dynamic_cast(_camera) != 0) { @@ -389,7 +569,7 @@ void CameraThread::mainLoopKill() } } -void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const +void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * info) const { UASSERT(dataPtr!=0); SensorData & data = *dataPtr; diff --git a/corelib/src/camera/CameraDepthAI.cpp b/corelib/src/camera/CameraDepthAI.cpp index e45a5f616a..ae0f781ccb 100644 --- a/corelib/src/camera/CameraDepthAI.cpp +++ b/corelib/src/camera/CameraDepthAI.cpp @@ -638,7 +638,7 @@ std::string CameraDepthAI::getSerial() const return ""; } -SensorData CameraDepthAI::captureImage(CameraInfo * info) +SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_DEPTHAI diff --git a/corelib/src/camera/CameraFreenect.cpp b/corelib/src/camera/CameraFreenect.cpp index 7159eb3a2b..9d48215f55 100644 --- a/corelib/src/camera/CameraFreenect.cpp +++ b/corelib/src/camera/CameraFreenect.cpp @@ -418,7 +418,7 @@ std::string CameraFreenect::getSerial() const return ""; } -SensorData CameraFreenect::captureImage(CameraInfo * info) +SensorData CameraFreenect::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_FREENECT diff --git a/corelib/src/camera/CameraFreenect2.cpp b/corelib/src/camera/CameraFreenect2.cpp index a1b1db1c6a..85f7bdf05e 100644 --- a/corelib/src/camera/CameraFreenect2.cpp +++ b/corelib/src/camera/CameraFreenect2.cpp @@ -334,7 +334,7 @@ std::string CameraFreenect2::getSerial() const return ""; } -SensorData CameraFreenect2::captureImage(CameraInfo * info) +SensorData CameraFreenect2::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_FREENECT2 diff --git a/corelib/src/camera/CameraImages.cpp b/corelib/src/camera/CameraImages.cpp index ca6e82071f..0591181aee 100644 --- a/corelib/src/camera/CameraImages.cpp +++ b/corelib/src/camera/CameraImages.cpp @@ -670,7 +670,7 @@ std::vector CameraImages::filenames() const return std::vector(); } -SensorData CameraImages::captureImage(CameraInfo * info) +SensorData CameraImages::captureImage(SensorCaptureInfo * info) { if(_syncImageRateWithStamps && _captureDelay>0.0) { diff --git a/corelib/src/camera/CameraK4A.cpp b/corelib/src/camera/CameraK4A.cpp index 78f72a5a9d..ad5714d744 100644 --- a/corelib/src/camera/CameraK4A.cpp +++ b/corelib/src/camera/CameraK4A.cpp @@ -424,7 +424,7 @@ std::string CameraK4A::getSerial() const #endif } -SensorData CameraK4A::captureImage(CameraInfo * info) +SensorData CameraK4A::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraK4W2.cpp b/corelib/src/camera/CameraK4W2.cpp index 5c3b272502..e289029e8a 100644 --- a/corelib/src/camera/CameraK4W2.cpp +++ b/corelib/src/camera/CameraK4W2.cpp @@ -278,7 +278,7 @@ std::string CameraK4W2::getSerial() const return ""; } -SensorData CameraK4W2::captureImage(CameraInfo * info) +SensorData CameraK4W2::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraMyntEye.cpp b/corelib/src/camera/CameraMyntEye.cpp index 9f1158c08b..01f6d12f1a 100644 --- a/corelib/src/camera/CameraMyntEye.cpp +++ b/corelib/src/camera/CameraMyntEye.cpp @@ -598,7 +598,7 @@ void CameraMyntEye::getPoseAndIMU( } #endif -SensorData CameraMyntEye::captureImage(CameraInfo * info) +SensorData CameraMyntEye::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_MYNTEYE diff --git a/corelib/src/camera/CameraOpenNI2.cpp b/corelib/src/camera/CameraOpenNI2.cpp index 786792ed12..8be86b74ca 100644 --- a/corelib/src/camera/CameraOpenNI2.cpp +++ b/corelib/src/camera/CameraOpenNI2.cpp @@ -473,7 +473,7 @@ std::string CameraOpenNI2::getSerial() const return ""; } -SensorData CameraOpenNI2::captureImage(CameraInfo * info) +SensorData CameraOpenNI2::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_OPENNI2 diff --git a/corelib/src/camera/CameraOpenNICV.cpp b/corelib/src/camera/CameraOpenNICV.cpp index 3956a8d2cc..13536d6f76 100644 --- a/corelib/src/camera/CameraOpenNICV.cpp +++ b/corelib/src/camera/CameraOpenNICV.cpp @@ -105,7 +105,7 @@ bool CameraOpenNICV::isCalibrated() const return true; } -SensorData CameraOpenNICV::captureImage(CameraInfo * info) +SensorData CameraOpenNICV::captureImage(SensorCaptureInfo * info) { SensorData data; if(_capture.isOpened()) diff --git a/corelib/src/camera/CameraOpenni.cpp b/corelib/src/camera/CameraOpenni.cpp index 95c876d402..9fb8dc7377 100644 --- a/corelib/src/camera/CameraOpenni.cpp +++ b/corelib/src/camera/CameraOpenni.cpp @@ -182,7 +182,7 @@ std::string CameraOpenni::getSerial() const return ""; } -SensorData CameraOpenni::captureImage(CameraInfo * info) +SensorData CameraOpenni::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_OPENNI diff --git a/corelib/src/camera/CameraRGBDImages.cpp b/corelib/src/camera/CameraRGBDImages.cpp index 0bc5d09510..7bcf83223d 100644 --- a/corelib/src/camera/CameraRGBDImages.cpp +++ b/corelib/src/camera/CameraRGBDImages.cpp @@ -70,7 +70,7 @@ bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::st return success; } -SensorData CameraRGBDImages::captureImage(CameraInfo * info) +SensorData CameraRGBDImages::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraRealSense.cpp b/corelib/src/camera/CameraRealSense.cpp index 2f0beb8134..3f545bb587 100644 --- a/corelib/src/camera/CameraRealSense.cpp +++ b/corelib/src/camera/CameraRealSense.cpp @@ -856,7 +856,7 @@ Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose) } #endif -SensorData CameraRealSense::captureImage(CameraInfo * info) +SensorData CameraRealSense::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_REALSENSE diff --git a/corelib/src/camera/CameraRealSense2.cpp b/corelib/src/camera/CameraRealSense2.cpp index c16e530c9f..9392873d60 100644 --- a/corelib/src/camera/CameraRealSense2.cpp +++ b/corelib/src/camera/CameraRealSense2.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include @@ -78,7 +77,6 @@ CameraRealSense2::CameraRealSense2( cameraDepthHeight_(480), cameraDepthFps_(30), globalTimeSync_(true), - publishInterIMU_(false), dualMode_(false), closing_(false) #endif @@ -138,12 +136,12 @@ void CameraRealSense2::imu_callback(rs2::frame frame) { auto stream = frame.get_profile().stream_type(); cv::Vec3f crnt_reading = *reinterpret_cast(frame.get_data()); - UDEBUG("%s callback! %f (%f %f %f)", - stream == RS2_STREAM_GYRO?"GYRO":"ACC", - frame.get_timestamp(), - crnt_reading[0], - crnt_reading[1], - crnt_reading[2]); + //UDEBUG("%s callback! %f (%f %f %f)", + // stream == RS2_STREAM_GYRO?"GYRO":"ACC", + // frame.get_timestamp(), + // crnt_reading[0], + // crnt_reading[1], + // crnt_reading[2]); UScopeMutex sm(imuMutex_); if(stream == RS2_STREAM_GYRO) { @@ -194,7 +192,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame) void CameraRealSense2::frame_callback(rs2::frame frame) { - UDEBUG("Frame callback! %f", frame.get_timestamp()); + //UDEBUG("Frame callback! %f", frame.get_timestamp()); syncer_(frame); } void CameraRealSense2::multiple_message_callback(rs2::frame frame) @@ -1139,14 +1137,14 @@ bool CameraRealSense2::odomProvided() const #endif } -bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance) +bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime) { #ifdef RTABMAP_REALSENSE2 IMU imu; unsigned int confidence = 0; double rsStamp = stamp*1000.0; Transform p; - getPoseAndIMU(rsStamp, p, confidence, imu); + getPoseAndIMU(rsStamp, p, confidence, imu, maxWaitTime*1000); if(!p.isNull()) { @@ -1202,13 +1200,6 @@ void CameraRealSense2::setGlobalTimeSync(bool enabled) #endif } -void CameraRealSense2::publishInterIMU(bool enabled) -{ -#ifdef RTABMAP_REALSENSE2 - publishInterIMU_ = enabled; -#endif -} - void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics) { #ifdef RTABMAP_REALSENSE2 @@ -1252,7 +1243,7 @@ void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled, #endif } -SensorData CameraRealSense2::captureImage(CameraInfo * info) +SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_REALSENSE2 @@ -1466,11 +1457,11 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence); info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence); } - if(!imu.empty() && !publishInterIMU_) + if(!imu.empty() && !isInterIMUPublishing()) { data.setIMU(imu); } - else if(publishInterIMU_ && !gyroBuffer_.empty()) + else if(isInterIMUPublishing() && !gyroBuffer_.empty()) { if(lastImuStamp_ > 0.0) { @@ -1501,7 +1492,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info) getPoseAndIMU(stamps[i], tmp, confidence, imuTmp); if(!imuTmp.empty()) { - UEventsManager::post(new IMUEvent(imuTmp, stamps[i]/1000.0)); + this->postInterIMU(imuTmp, stamps[i]/1000.0); pub++; } else diff --git a/corelib/src/camera/CameraStereoDC1394.cpp b/corelib/src/camera/CameraStereoDC1394.cpp index b82b591ea1..691516b73c 100644 --- a/corelib/src/camera/CameraStereoDC1394.cpp +++ b/corelib/src/camera/CameraStereoDC1394.cpp @@ -396,7 +396,7 @@ std::string CameraStereoDC1394::getSerial() const return ""; } -SensorData CameraStereoDC1394::captureImage(CameraInfo * info) +SensorData CameraStereoDC1394::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_DC1394 diff --git a/corelib/src/camera/CameraStereoFlyCapture2.cpp b/corelib/src/camera/CameraStereoFlyCapture2.cpp index 8153beb4b4..ca95cc87fb 100644 --- a/corelib/src/camera/CameraStereoFlyCapture2.cpp +++ b/corelib/src/camera/CameraStereoFlyCapture2.cpp @@ -260,7 +260,7 @@ std::string CameraStereoFlyCapture2::getSerial() const return ""; } -SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info) +SensorData CameraStereoFlyCapture2::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_FLYCAPTURE2 diff --git a/corelib/src/camera/CameraStereoImages.cpp b/corelib/src/camera/CameraStereoImages.cpp index 2ceed75d3e..0a7d3915d2 100644 --- a/corelib/src/camera/CameraStereoImages.cpp +++ b/corelib/src/camera/CameraStereoImages.cpp @@ -156,7 +156,7 @@ std::string CameraStereoImages::getSerial() const return stereoModel_.name(); } -SensorData CameraStereoImages::captureImage(CameraInfo * info) +SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraStereoTara.cpp b/corelib/src/camera/CameraStereoTara.cpp index bde7b89db9..e4ea3cdfd1 100644 --- a/corelib/src/camera/CameraStereoTara.cpp +++ b/corelib/src/camera/CameraStereoTara.cpp @@ -136,7 +136,7 @@ std::string CameraStereoTara::getSerial() const return cameraName_; } -SensorData CameraStereoTara::captureImage(CameraInfo * info) +SensorData CameraStereoTara::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraStereoVideo.cpp b/corelib/src/camera/CameraStereoVideo.cpp index c109136300..392e1c4898 100644 --- a/corelib/src/camera/CameraStereoVideo.cpp +++ b/corelib/src/camera/CameraStereoVideo.cpp @@ -243,7 +243,7 @@ std::string CameraStereoVideo::getSerial() const return cameraName_; } -SensorData CameraStereoVideo::captureImage(CameraInfo * info) +SensorData CameraStereoVideo::captureImage(SensorCaptureInfo * info) { SensorData data; diff --git a/corelib/src/camera/CameraStereoZed.cpp b/corelib/src/camera/CameraStereoZed.cpp index 4a9ff233dc..15416afa1e 100644 --- a/corelib/src/camera/CameraStereoZed.cpp +++ b/corelib/src/camera/CameraStereoZed.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #ifdef RTABMAP_ZED @@ -167,12 +166,13 @@ IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTr class ZedIMUThread: public UThread { public: - ZedIMUThread(float rate, sl::Camera * zed, const Transform & imuLocalTransform, bool accurate) + ZedIMUThread(float rate, sl::Camera * zed, CameraStereoZed * camera, const Transform & imuLocalTransform, bool accurate) { UASSERT(rate > 0.0f); - UASSERT(zed != 0); + UASSERT(zed != 0 && camera != 0); rate_ = rate; zed_= zed; + camera_ = camera; accurate_ = accurate; imuLocalTransform_ = imuLocalTransform; } @@ -212,19 +212,20 @@ class ZedIMUThread: public UThread bool res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE); if(res == sl::SUCCESS && imudata.valid) { - UEventsManager::post(new IMUEvent(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now())); + this->postInterIMU(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now()); } #else sl::SensorsData sensordata; - sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::IMAGE); + sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::CURRENT); if(res == sl::ERROR_CODE::SUCCESS && sensordata.imu.is_available) { - UEventsManager::post(new IMUEvent(zedIMUtoIMU(sensordata, imuLocalTransform_), UTimer::now())); + camera_->postInterIMUPublic(zedIMUtoIMU(sensordata, imuLocalTransform_), double(sensordata.imu.timestamp)/10e9); } #endif } float rate_; sl::Camera * zed_; + CameraStereoZed * camera_; bool accurate_; Transform imuLocalTransform_; UTimer frameRateTimer_; @@ -279,7 +280,6 @@ CameraStereoZed::CameraStereoZed( computeOdometry_(computeOdometry), lost_(true), force3DoF_(odomForce3DoF), - publishInterIMU_(false), imuPublishingThread_(0) #endif { @@ -345,7 +345,6 @@ CameraStereoZed::CameraStereoZed( computeOdometry_(computeOdometry), lost_(true), force3DoF_(odomForce3DoF), - publishInterIMU_(false), imuPublishingThread_(0) #endif { @@ -386,13 +385,6 @@ CameraStereoZed::~CameraStereoZed() #endif } -void CameraStereoZed::publishInterIMU(bool enabled) -{ -#ifdef RTABMAP_ZED - publishInterIMU_ = enabled; -#endif -} - bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName) { UDEBUG(""); @@ -564,9 +556,9 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str imuLocalTransform_.prettyPrint().c_str(), zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str()); #endif - if(publishInterIMU_) + if(isInterIMUPublishing()) { - imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true); + imuPublishingThread_ = new ZedIMUThread(200, zed_, this, imuLocalTransform_, true); imuPublishingThread_->start(); } } @@ -607,7 +599,7 @@ bool CameraStereoZed::odomProvided() const #endif } -bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance) +bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime) { #ifdef RTABMAP_ZED @@ -683,7 +675,7 @@ bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covarian return false; } -SensorData CameraStereoZed::captureImage(CameraInfo * info) +SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_ZED @@ -711,10 +703,12 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) #else sl::ERROR_CODE res; + sl::Timestamp timestamp; bool imuReceived = true; do { res = zed_->grab(rparam); + timestamp = zed_->getTimestamp(sl::TIME_REFERENCE::IMAGE); // If the sensor supports IMU, wait IMU to be available before sending data. if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull()) @@ -752,8 +746,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) zed_->retrieveMeasure(tmp,sl::MEASURE::DEPTH); #endif slMat2cvMat(tmp).copyTo(depth); - +#if ZED_SDK_MAJOR_VERSION < 3 data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now()); +#else + data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), double(timestamp)/10e9); +#endif } else { @@ -766,8 +763,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) cv::Mat rgbaRight = slMat2cvMat(tmp); cv::Mat right; cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY); - +#if ZED_SDK_MAJOR_VERSION < 3 data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now()); +#else + data = SensorData(left, right, stereoModel_, this->getNextSeqID(), double(timestamp)/10e9); +#endif } if(imuPublishingThread_ == 0) @@ -803,6 +803,13 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) info->odomPose = zedPoseToTransform(pose); if (!info->odomPose.isNull()) { +#if ZED_SDK_MAJOR_VERSION >=3 + if(pose.timestamp != timestamp) + { + UWARN("Pose retrieve doesn't have same stamp (%ld) than grabbed image (%ld)", pose.timestamp, timestamp); + } +#endif + //transform from: // x->right, y->down, z->forward //to: @@ -858,4 +865,9 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) return data; } +void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp) +{ + postInterIMU(imu, stamp); +} + } // namespace rtabmap diff --git a/corelib/src/camera/CameraStereoZedOC.cpp b/corelib/src/camera/CameraStereoZedOC.cpp index bcceae1e96..5b961fe33f 100644 --- a/corelib/src/camera/CameraStereoZedOC.cpp +++ b/corelib/src/camera/CameraStereoZedOC.cpp @@ -710,7 +710,7 @@ std::string CameraStereoZedOC::getSerial() const return ""; } -SensorData CameraStereoZedOC::captureImage(CameraInfo * info) +SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info) { SensorData data; #ifdef RTABMAP_ZEDOC diff --git a/corelib/src/camera/CameraVideo.cpp b/corelib/src/camera/CameraVideo.cpp index 53287da1ee..39b09909e0 100644 --- a/corelib/src/camera/CameraVideo.cpp +++ b/corelib/src/camera/CameraVideo.cpp @@ -162,7 +162,7 @@ std::string CameraVideo::getSerial() const return _guid; } -SensorData CameraVideo::captureImage(CameraInfo * info) +SensorData CameraVideo::captureImage(SensorCaptureInfo * info) { cv::Mat img; if(_capture.isOpened()) diff --git a/corelib/src/lidar/LidarVLP16.cpp b/corelib/src/lidar/LidarVLP16.cpp new file mode 100644 index 0000000000..2fa758a0ef --- /dev/null +++ b/corelib/src/lidar/LidarVLP16.cpp @@ -0,0 +1,378 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include +#include + +#if PCL_VERSION_COMPARE(<, 1, 9, 0) +#define VLP_MAX_NUM_LASERS 16 +#define VLP_DUAL_MODE 0x39 +#endif + +namespace rtabmap { + +/** @brief Function used to check that hour assigned to timestamp in conversion is + * correct. Velodyne only returns time since the top of the hour, so if the computer clock + * and the velodyne clock (gps-synchronized) are a little off, there is a chance the wrong + * hour may be associated with the timestamp + * + * Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley + * Original license: BSD License 2.0 + * Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp + * + * @param stamp timestamp recovered from velodyne + * @param nominal_stamp time coming from computer's clock + * @return timestamp from velodyne, possibly shifted by 1 hour if the function arguments + * disagree by more than a half-hour. + */ +double resolveHourAmbiguity(const double &stamp, const double &nominal_stamp) { + const int HALFHOUR_TO_SEC = 1800; + double retval = stamp; + if (nominal_stamp > stamp) { + if (nominal_stamp - stamp > HALFHOUR_TO_SEC) { + retval = retval + 2*HALFHOUR_TO_SEC; + } + } else if (stamp - nominal_stamp > HALFHOUR_TO_SEC) { + retval = retval - 2*HALFHOUR_TO_SEC; + } + return retval; +} + +/* + * Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley + * Original license: BSD License 2.0 + * Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp + */ +double rosTimeFromGpsTimestamp(const uint32_t data) { + const int HOUR_TO_SEC = 3600; + // time for each packet is a 4 byte uint + // It is the number of microseconds from the top of the hour + double time_nom = UTimer::now(); + uint32_t cur_hour = time_nom / HOUR_TO_SEC; + double stamp = double(cur_hour * HOUR_TO_SEC) + double(data) / 1000000; + stamp = resolveHourAmbiguity(stamp, time_nom); + return stamp; +} + +LidarVLP16::LidarVLP16( + const std::string& pcapFile, + bool organized, + bool stampLast, + float frameRate, + Transform localTransform) : + Lidar(frameRate, localTransform), + pcl::VLPGrabber(pcapFile), + timingOffsetsDualMode_(false), + startSweepTime_(0), + startSweepTimeHost_(0), + organized_(organized), + useHostTime_(false), + stampLast_(stampLast) +{ + UDEBUG("Using PCAP file \"%s\"", pcapFile.c_str()); +} +LidarVLP16::LidarVLP16( + const boost::asio::ip::address& ipAddress, + const std::uint16_t port, + bool organized, + bool useHostTime, + bool stampLast, + float frameRate, + Transform localTransform) : + Lidar(frameRate, localTransform), + pcl::VLPGrabber(ipAddress, port), + timingOffsetsDualMode_(false), + startSweepTime_(0), + startSweepTimeHost_(0), + organized_(organized), + useHostTime_(useHostTime), + stampLast_(stampLast) +{ + UDEBUG("Using network lidar with IP=%s port=%d", ipAddress.to_string().c_str(), port); +} + +LidarVLP16::~LidarVLP16() +{ + UDEBUG("Stopping lidar..."); + stop(); + scanReady_.release(); + UDEBUG("Stopped lidar!"); +} + +void LidarVLP16::setOrganized(bool enable) +{ + organized_ = true; +} + +bool LidarVLP16::init(const std::string &, const std::string &) +{ + UDEBUG("Init lidar"); + if(isRunning()) + { + UDEBUG("Stopping lidar..."); + stop(); + uSleep(2000); // make sure all callbacks are finished + UDEBUG("Stopped lidar!"); + } + startSweepTime_ = 0.0; + startSweepTimeHost_ = 0.0; + accumulatedScans_.clear(); + if(organized_) + { + accumulatedScans_.resize(16); + } + else + { + accumulatedScans_.resize(1); + } + buildTimings(false); + start(); + UDEBUG("Lidar capture started"); + return true; +} + +/** + * Build a timing table for each block/firing. Stores in timing_offsets vector + */ +void LidarVLP16::buildTimings(bool dualMode) +{ + // vlp16 + // timing table calculation, from velodyne user manual + timingOffsets_.resize(12); + for (size_t i=0; i < timingOffsets_.size(); ++i){ + timingOffsets_[i].resize(32); + } + // constants + double full_firing_cycle = 55.296 * 1e-6; // seconds + double single_firing = 2.304 * 1e-6; // seconds + double dataBlockIndex, dataPointIndex; + // compute timing offsets + for (size_t x = 0; x < timingOffsets_.size(); ++x){ + for (size_t y = 0; y < timingOffsets_[x].size(); ++y){ + if (dualMode){ + dataBlockIndex = (x - (x % 2)) + (y / 16); + } + else{ + dataBlockIndex = (x * 2) + (y / 16); + } + dataPointIndex = y % 16; + //timing_offsets[block][firing] + timingOffsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex); + } + } + timingOffsetsDualMode_ = dualMode; +} + +void LidarVLP16::toPointClouds (HDLDataPacket *dataPacket) +{ + if (sizeof(HDLLaserReturn) != 3) + return; + + double receivedHostTime = UTimer::now(); + double packetStamp = rosTimeFromGpsTimestamp(dataPacket->gpsTimestamp); + if(startSweepTime_==0) + { + startSweepTime_ = packetStamp; + startSweepTimeHost_ = receivedHostTime; + } + + bool dualMode = dataPacket->mode == VLP_DUAL_MODE; + if(timingOffsets_.empty() || timingOffsetsDualMode_ != dualMode) + { + // reset everything + timingOffsets_.clear(); + buildTimings(dualMode); + startSweepTime_ = packetStamp; + startSweepTimeHost_ = receivedHostTime; + for(size_t i=0; ifiringData[index].rotationalPosition < dataPacket->firingData[0].rotationalPosition) + { + interpolated_azimuth_delta = ((dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0; + } + else + { + interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0; + } + + for (std::uint8_t i = 0; i < HDL_FIRING_PER_PKT; ++i) + { + HDLFiringData firing_data = dataPacket->firingData[i]; + + for (std::uint8_t j = 0; j < HDL_LASER_PER_FIRING; j++) + { + double current_azimuth = firing_data.rotationalPosition; + if (j >= VLP_MAX_NUM_LASERS) + { + current_azimuth += interpolated_azimuth_delta; + } + if (current_azimuth > 36000) + { + current_azimuth -= 36000; + } + + double t = 0; + if (timingOffsets_.size()) + t = timingOffsets_[i][j]; + + if (current_azimuth < HDLGrabber::last_azimuth_) + { + if (!accumulatedScans_[0].empty()) + { + UScopeMutex lock(lastScanMutex_); + bool notify = lastScan_.laserScanRaw().empty(); + if(stampLast_) + { + double lastStamp = startSweepTime_ + accumulatedScans_[accumulatedScans_.size()-1].back().t; + double diff = lastStamp - startSweepTime_; + lastScan_.setStamp(useHostTime_?startSweepTimeHost_+diff:lastStamp); + for(size_t r=0; r 1) + { + cv::Mat organizedScan = cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone(); + for(size_t k=1; k1) + { + accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back(xyzit); + } + else if (! (std::isnan (xyzit.x) || std::isnan (xyzit.y) || std::isnan (xyzit.z))) + { + accumulatedScans_[0].push_back (xyzit); + } + last_azimuth_ = current_azimuth; + + if (dualMode) + { + pcl::PointXYZI dual_xyzi; + HDLGrabber::computeXYZI (dual_xyzi, current_azimuth, dataPacket->firingData[i + 1].laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]); + + if(accumulatedScans_.size()>1) + { + xyzit.x = dual_xyzi.y; + xyzit.y = -dual_xyzi.x; + xyzit.z = dual_xyzi.z; + xyzit.i = dual_xyzi.intensity; + xyzit.t = timeSinceStartOfThisScan; + accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back (xyzit); + } + else if ((dual_xyzi.x != xyzi.x || dual_xyzi.y != xyzi.y || dual_xyzi.z != xyzi.z) + && ! (std::isnan (dual_xyzi.x) || std::isnan (dual_xyzi.y) || std::isnan (dual_xyzi.z))) + { + xyzit.x = dual_xyzi.y; + xyzit.y = -dual_xyzi.x; + xyzit.z = dual_xyzi.z; + xyzit.i = dual_xyzi.intensity; + xyzit.t = timeSinceStartOfThisScan; + accumulatedScans_[0].push_back (xyzit); + } + } + } + if (dualMode) + { + i++; + } + } +} + +SensorData LidarVLP16::captureData(SensorCaptureInfo * info) +{ + SensorData data; + if(scanReady_.acquire(1, 5000)) + { + UScopeMutex lock(lastScanMutex_); + if(!lastScan_.laserScanRaw().empty()) + { + data = lastScan_; + lastScan_ = SensorData(); + } + } + else + { + UWARN("Did not receive any scans for the past 5 seconds."); + } + return data; +} + + +} /* namespace rtabmap */ diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 8024c03676..6239a8d0aa 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -156,11 +156,14 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : regPipeline_ = Registration::create(bundleParameters); if(bundleAdjustment_>0 && regPipeline_->isScanRequired()) { - UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.", - Parameters::kOdomF2MBundleAdjustment().c_str(), - bundleAdjustment_, - Parameters::kRegStrategy().c_str(), - uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str()); + if(regPipeline_->isImageRequired()) + { + UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.", + Parameters::kOdomF2MBundleAdjustment().c_str(), + bundleAdjustment_, + Parameters::kRegStrategy().c_str(), + uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str()); + } bundleAdjustment_ = 0; } diff --git a/corelib/src/pcl18/surface/impl/texture_mapping.hpp b/corelib/src/pcl18/surface/impl/texture_mapping.hpp index bb31f4b99d..98ea32ffc8 100644 --- a/corelib/src/pcl18/surface/impl/texture_mapping.hpp +++ b/corelib/src/pcl18/surface/impl/texture_mapping.hpp @@ -975,10 +975,6 @@ pcl::TextureMapping::textureMeshwithMultipleCameras (pcl::TextureMesh visible_faces.resize (cpt_visible_faces); mesh.tex_polygons[current_cam].clear (); mesh.tex_polygons[current_cam] = visible_faces; - - int nb_faces = 0; - for (int i = 0; i < static_cast (mesh.tex_polygons.size ()); i++) - nb_faces += static_cast (mesh.tex_polygons[i].size ()); } // we have been through all the cameras. diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp index 3166e531f9..606dfa6996 100644 --- a/corelib/src/util3d.cpp +++ b/corelib/src/util3d.cpp @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -2238,7 +2239,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con { pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud); } - else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI) + else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT) { pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud); } @@ -2268,8 +2269,17 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con pcl::PointCloud::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::Ptr laserScanToPointCloud(const LaserScan & lase pcl::PointCloud::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull(); for(int i=0; i::Ptr laserScanToPointCloudNormal(const LaserSc pcl::PointCloud::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::Ptr laserScanToPointCloudRGB(const LaserScan pcl::PointCloud::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform, float intensity) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); Eigen::Affine3f transform3f = transform.toEigen3f(); for(int i=0; i::Ptr laserScanToPointCloudI(const LaserScan & la pcl::PointCloud::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); for(int i=0; i::Ptr laserScanToPointCloudRGBNormal(cons pcl::PointCloud::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform, float intensity) { pcl::PointCloud::Ptr output(new pcl::PointCloud); + if(laserScan.isOrganized()) + { + output->width = laserScan.data().cols; + output->height = laserScan.data().rows; + output->is_dense = false; + } + else + { + output->is_dense = true; + } output->resize(laserScan.size()); - output->is_dense = true; bool nullTransform = transform.isNull() || transform.isIdentity(); for(int i=0; i(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2388,7 +2444,8 @@ pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index) { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointNormal output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2409,7 +2466,8 @@ pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, uns { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointXYZRGB output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2448,7 +2506,8 @@ pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float i { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointXYZI output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2473,7 +2532,8 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, in { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointXYZRGBNormal output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -2520,7 +2580,8 @@ pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int in { UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size()); pcl::PointXYZINormal output; - const float * ptr = laserScan.data().ptr(0, index); + int row = index / laserScan.data().cols; + const float * ptr = laserScan.data().ptr(row, index - row*laserScan.data().cols); output.x = ptr[0]; output.y = ptr[1]; if(!laserScan.is2d()) @@ -3489,6 +3550,162 @@ pcl::PointCloud::Ptr loadCloud( return util3d::transformPointCloud(cloud, transform); } +LaserScan deskew( + const LaserScan & input, + double inputStamp, + const rtabmap::Transform & velocity) +{ + if(velocity.isNull()) + { + UERROR("velocity should be valid!"); + return LaserScan(); + } + + if(input.format() != LaserScan::kXYZIT) + { + UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str()); + return LaserScan(); + } + + if(input.empty()) + { + UERROR("input scan is empty!"); + return LaserScan(); + } + + int offsetTime = input.getTimeOffset(); + + // Get latest timestamp + double firstStamp; + double lastStamp; + firstStamp = inputStamp + input.data().ptr(0, 0)[offsetTime]; + lastStamp = inputStamp + input.data().ptr(0, input.size()-1)[offsetTime]; + + if(lastStamp <= firstStamp) + { + UERROR("First and last stamps in the scan are the same!"); + return LaserScan(); + } + + rtabmap::Transform firstPose; + rtabmap::Transform lastPose; + + float vx,vy,vz, vroll,vpitch,vyaw; + velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw); + + // 1- The pose of base frame in odom frame at first stamp + // 2- The pose of base frame in odom frame at last stamp + double dt1 = firstStamp - inputStamp; + double dt2 = lastStamp - inputStamp; + + firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1); + lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2); + + if(firstPose.isNull()) + { + UERROR("Could not get transform between stamps %f and %f!", + firstStamp, + inputStamp); + return LaserScan(); + } + if(lastPose.isNull()) + { + UERROR("Could not get transform between stamps %f and %f!", + lastStamp, + inputStamp); + return LaserScan(); + } + + double stamp; + UTimer processingTime; + double scanTime = lastStamp - firstStamp; + cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense + int offsetIntensity = input.getIntensityOffset(); + bool isLocalTransformIdentity = input.localTransform().isIdentity(); + Transform localTransformInv = input.localTransform().inverse(); + + bool timeOnColumns = input.data().cols > input.data().rows; + int oi = 0; + if(timeOnColumns) + { + // t1 t2 ... + // ring1 ring1 ... + // ring2 ring2 ... + // ring3 ring4 ... + // ring4 ring3 ... + for(int u=0; u(0, u); + stamp = inputStamp + inputPtr[offsetTime]; + rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose); + + for(int v=0; v(v, u); + pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]); + if(pcl::isFinite(pt)) + { + if(!isLocalTransformIdentity) + { + pt = rtabmap::util3d::transformPoint(pt, input.localTransform()); + } + pt = rtabmap::util3d::transformPoint(pt, transform); + if(!isLocalTransformIdentity) + { + pt = rtabmap::util3d::transformPoint(pt, localTransformInv); + } + float * dataPtr = output.ptr(0, oi++); + dataPtr[0] = pt.x; + dataPtr[1] = pt.y; + dataPtr[2] = pt.z; + dataPtr[3] = input.data().ptr(v, u)[offsetIntensity]; + } + } + } + } + else // time on rows + { + // t1 ring1 ring2 ring3 ring4 + // t2 ring1 ring2 ring3 ring4 + // t3 ring1 ring2 ring3 ring4 + // t4 ring1 ring2 ring3 ring4 + // ... ... ... ... ... + for(int v=0; v(v, 0); + stamp = inputStamp + inputPtr[offsetTime]; + rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose); + + for(int u=0; u(v, u); + pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]); + if(pcl::isFinite(pt)) + { + if(!isLocalTransformIdentity) + { + pt = rtabmap::util3d::transformPoint(pt, input.localTransform()); + } + pt = rtabmap::util3d::transformPoint(pt, transform); + if(!isLocalTransformIdentity) + { + pt = rtabmap::util3d::transformPoint(pt, localTransformInv); + } + float * dataPtr = output.ptr(0, oi++); + dataPtr[0] = pt.x; + dataPtr[1] = pt.y; + dataPtr[2] = pt.z; + dataPtr[3] = input.data().ptr(v, u)[offsetIntensity]; + } + } + } + } + output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi)); + UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed()); + return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform()); +} + + } } diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index c32ffb304c..134cb6a235 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -82,8 +82,8 @@ LaserScan commonFiltering( float groundNormalsUp) { LaserScan scan = scanIn; - UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f", - scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp); + UDEBUG("scan size=%d format=%d, organized=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f", + scan.size(), (int)scan.format(), scan.isOrganized()?1:0, downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp); if(!scan.isEmpty()) { // combined downsampling and range filtering step @@ -99,34 +99,44 @@ LaserScan commonFiltering( int oi = 0; float rangeMinSqrd = rangeMin * rangeMin; float rangeMaxSqrd = rangeMax * rangeMax; - for(int i=0; i scan.data().cols?downsamplingStep:1; + int downsamplingCols = scan.data().cols > scan.data().rows?downsamplingStep:1; + for(int j=0; j(0, i); - - if(rangeMin>0.0f || rangeMax>0.0f) + for(int i=0; i(j, i); - if(rangeMin > 0.0f && r < rangeMinSqrd) + if(rangeMin>0.0f || rangeMax>0.0f) { - continue; - } - if(rangeMax > 0.0f && r > rangeMaxSqrd) - { - continue; + float r; + if(is2d) + { + r = ptr[0]*ptr[0] + ptr[1]*ptr[1]; + } + else + { + r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2]; + } + + if(!uIsFinite(r)) + { + continue; + } + + if(rangeMin > 0.0f && r < rangeMinSqrd) + { + continue; + } + if(rangeMax > 0.0f && r > rangeMaxSqrd) + { + continue; + } } - } - cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1))); - ++oi; + cv::Mat(scan.data(), cv::Range(j,j+1), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1))); + ++oi; + } } int previousSize = scan.size(); int scanMaxPtsTmp = scan.maxPoints(); diff --git a/corelib/src/util3d_mapping.cpp b/corelib/src/util3d_mapping.cpp index 0e5c3928f6..f46dd6b87d 100644 --- a/corelib/src/util3d_mapping.cpp +++ b/corelib/src/util3d_mapping.cpp @@ -655,7 +655,6 @@ cv::Mat create2DMap(const std::map & poses, map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1; UDEBUG("map size = %dx%d", map.cols, map.rows); - int j=0; float scanMaxRangeSqr = scanMaxRange * scanMaxRange; for(std::map >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) { @@ -741,14 +740,12 @@ cv::Mat create2DMap(const std::map & poses, } } } - ++j; } UDEBUG("Ray trace known space=%fs", timer.ticks()); // now fill unknown spaces if(unknownSpaceFilled && scanMaxRange > 0) { - j=0; float angleIncrement = CV_PI/90.0f; // angle increment for(std::map >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) { @@ -813,7 +810,6 @@ cv::Mat create2DMap(const std::map & poses, } } } - ++j; } UDEBUG("Fill empty space=%fs", timer.ticks()); //cv::imwrite("map.png", util3d::convertMap2Image8U(map)); diff --git a/corelib/src/util3d_surface.cpp b/corelib/src/util3d_surface.cpp index 2dca472b40..adfc1248c5 100644 --- a/corelib/src/util3d_surface.cpp +++ b/corelib/src/util3d_surface.cpp @@ -3518,22 +3518,25 @@ LaserScan adjustNormalsToViewPoint( int nz = ny+1; cv::Mat output = scan.data().clone(); #pragma omp parallel for - for(int i=0; i(0, i); - if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])) + for(int i=0; i0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground + float * ptr = output.ptr(j, i); + if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])) { - //reverse normal - ptr[nx] *= -1.0f; - ptr[ny] *= -1.0f; - ptr[nz] *= -1.0f; + Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]); + Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]); + + float result = v.dot(n); + if(result < 0 + || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground + { + //reverse normal + ptr[nx] *= -1.0f; + ptr[ny] *= -1.0f; + ptr[nz] *= -1.0f; + } } } } diff --git a/data/presets/camera_tof_icp.ini b/data/presets/camera_tof_icp.ini new file mode 100644 index 0000000000..84a09c1fd0 --- /dev/null +++ b/data/presets/camera_tof_icp.ini @@ -0,0 +1,31 @@ +# Could be used with TOF cameras like Kinect v2, Kinect For Azure or L515 + +[Camera] +Scan\downsampleStep = 4 +Scan\fromDepth = true +Scan\normalsK = 20 +Scan\normalsRadius = 0 +Scan\normalsUp = false +Scan\rangeMax = 0 +Scan\rangeMin = 0 +Scan\voxelSize = 0.05 + +[Gui] +General\showClouds0 = false +General\showClouds1 = false + +[Core] +Icp\CorrespondenceRatio = 0.2 +Icp\Epsilon = 0.005 +Icp\OutlierRatio = 0.65 +Icp\PointToPlaneMinComplexity = 0 +Icp\Strategy = 0 +Icp\VoxelSize = 0 +Odom\Deskewing = false +Odom\ScanKeyFrameThr = 0.7 +OdomF2M\ScanMaxSize = 15000 +# match the input voxel size: +OdomF2M\ScanSubtractRadius = 0.05 +RGBD\ProximityPathMaxNeighbors = 1 +# Make sure PM is used: +Reg\Strategy = 1 diff --git a/data/presets/lidar3d_icp.ini b/data/presets/lidar3d_icp.ini new file mode 100644 index 0000000000..57caca42a5 --- /dev/null +++ b/data/presets/lidar3d_icp.ini @@ -0,0 +1,46 @@ +# Could be used with LiDARs like Velodyne, RoboSense, Ouster + +[Camera] +Scan\downsampleStep = 1 +Scan\fromDepth = false +Scan\normalsK = 0 +Scan\normalsRadius = 0 +Scan\normalsUp = false +Scan\rangeMax = 0 +Scan\rangeMin = 0 +Scan\voxelSize = 0 + +[Gui] +General\showClouds0 = false +General\showClouds1 = false + +[Core] +# Would be 0.01 for odom and 0.2 for mapping: +Icp\CorrespondenceRatio = 0.1 +Icp\Epsilon = 0.001 +Icp\FiltersEnabled = 2 +Icp\Iterations = 10 +Icp\OutlierRatio = 0.7 +# ~10x the voxel size: +Icp\MaxCorrespondenceDistance = 0.5 +Icp\MaxTranslation = 2 +# Uncomment if lidar can see ground most of the time (on a car or wheeled robot): +#Icp\PointToPlaneGroundNormalsUp = 0.8 +Icp\PointToPlaneK = 20 +Icp\PointToPlaneMinComplexity = 0 +# Make sure PM is used: +Icp\Strategy = 1 +Icp\VoxelSize = 0.05 +Mem\NotLinkedNodesKept = false +Mem\STMSize = 30 +Odom\Deskewing = true +Odom\GuessSmoothingDelay = 0.3 +Odom\ScanKeyFrameThr = 0.6 +OdomF2M\ScanMaxSize = 15000 +# Match voxel size: +OdomF2M\ScanSubtractRadius = 0.05 +RGBD\AngularUpdate = 0.05 +RGBD\LinearUpdate = 0.05 +RGBD\ProximityMaxGraphDepth = 0 +RGBD\ProximityPathMaxNeighbors = 1 +Reg\Strategy = 1 diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e619619cd1..5e4a68a218 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -5,6 +5,7 @@ IF(TARGET rtabmap_gui) ADD_SUBDIRECTORY( RGBDMapping ) ADD_SUBDIRECTORY( WifiMapping ) ADD_SUBDIRECTORY( NoEventsExample ) + ADD_SUBDIRECTORY( LidarMapping ) ELSE() MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...") ENDIF() diff --git a/examples/LidarMapping/CMakeLists.txt b/examples/LidarMapping/CMakeLists.txt new file mode 100644 index 0000000000..4f88a87219 --- /dev/null +++ b/examples/LidarMapping/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.8) + +IF(DEFINED PROJECT_NAME) + set(internal TRUE) +ENDIF(DEFINED PROJECT_NAME) + +if(NOT internal) + # external build + PROJECT( MyProject ) + + FIND_PACKAGE(RTABMap REQUIRED COMPONENTS gui) + +endif() + +IF(QT4_FOUND OR Qt5_FOUND OR Qt6_FOUND) + SET(moc_srcs MapBuilder.h) +ENDIF() + +ADD_EXECUTABLE(lidar_mapping main.cpp ${moc_srcs}) + +TARGET_LINK_LIBRARIES(lidar_mapping rtabmap::gui) + +SET_TARGET_PROPERTIES( + lidar_mapping + PROPERTIES + AUTOUIC ON + AUTOMOC ON + AUTORCC ON +) + +if(internal) + SET_TARGET_PROPERTIES( lidar_mapping + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidar_mapping) +endif(internal) diff --git a/examples/LidarMapping/MapBuilder.h b/examples/LidarMapping/MapBuilder.h new file mode 100644 index 0000000000..b6e6b5ded5 --- /dev/null +++ b/examples/LidarMapping/MapBuilder.h @@ -0,0 +1,340 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef MAPBUILDER_H_ +#define MAPBUILDER_H_ + +#include +#include +#include + +#ifndef Q_MOC_RUN // Mac OS X issue +#include "rtabmap/gui/CloudViewer.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_filtering.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/core/RtabmapEvent.h" +#include "rtabmap/core/OccupancyGrid.h" +#endif +#include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UConversion.h" +#include "rtabmap/utilite/UEventsHandler.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/core/OdometryEvent.h" +#include + +using namespace rtabmap; + +// This class receives RtabmapEvent and construct/update a 3D Map +class MapBuilder : public QWidget, public UEventsHandler +{ + Q_OBJECT +public: + //Camera ownership is not transferred! + MapBuilder(SensorCaptureThread * camera = 0) : + sensorCaptureThread_(camera), + odometryCorrection_(Transform::getIdentity()), + processingStatistics_(false), + lastOdometryProcessed_(true), + visibility_(0) + { + this->setWindowFlags(Qt::Dialog); + this->setWindowTitle(tr("3D Map")); + this->setMinimumWidth(800); + this->setMinimumHeight(600); + + cloudViewer_ = new CloudViewer(this); + cloudViewer_->setCameraTargetLocked(true); + + QVBoxLayout *layout = new QVBoxLayout(); + layout->addWidget(cloudViewer_); + this->setLayout(layout); + + qRegisterMetaType("rtabmap::OdometryEvent"); + qRegisterMetaType("rtabmap::Statistics"); + + QAction * pause = new QAction(this); + this->addAction(pause); + pause->setShortcut(Qt::Key_Space); + connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection())); + + QAction * visibility = new QAction(this); + this->addAction(visibility); + visibility->setShortcut(Qt::Key_Tab); + connect(visibility, SIGNAL(triggered()), this, SLOT(rotateVisibility())); + } + + virtual ~MapBuilder() + { + this->unregisterFromEventsManager(); + } + +protected Q_SLOTS: + virtual void pauseDetection() + { + UWARN(""); + if(sensorCaptureThread_) + { + if(sensorCaptureThread_->isCapturing()) + { + sensorCaptureThread_->join(true); + } + else + { + sensorCaptureThread_->start(); + } + } + } + + virtual void rotateVisibility() + { + visibility_ = (visibility_+1) % 3; + if(visibility_ == 0) + { + UWARN("Show both Odom and Map"); + } + else if(visibility_ == 1) + { + UWARN("Show only Map"); + } + else if(visibility_ == 2) + { + UWARN("Show only Odom"); + } + } + + virtual void processOdometry(const rtabmap::OdometryEvent & odom) + { + if(!this->isVisible()) + { + return; + } + + Transform pose = odom.pose(); + if(pose.isNull()) + { + //Odometry lost + cloudViewer_->setBackgroundColor(Qt::darkRed); + + pose = lastOdomPose_; + } + else + { + cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor()); + } + if(!pose.isNull()) + { + lastOdomPose_ = pose; + + // 3d cloud + if(!odom.data().laserScanRaw().empty()) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw(), odom.data().laserScanRaw().localTransform()); + if(cloud->size() && (visibility_ == 0 || visibility_ == 2)) + { + if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose, Qt::magenta)) + { + UERROR("Adding cloudOdom to viewer failed!"); + } + } + else + { + cloudViewer_->setCloudVisibility("cloudOdom", false); + if(cloud->empty()) + UWARN("Empty cloudOdom!"); + } + } + + if(!odom.info().localScanMap.empty()) + { + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloud(odom.info().localScanMap, odom.info().localScanMap.localTransform()); + if(cloud->size() && (visibility_ == 0 || visibility_ == 2)) + { + if(!cloudViewer_->addCloud("cloudOdomLocalMap", cloud, odometryCorrection_, Qt::blue)) + { + UERROR("Adding cloudOdomLocalMap to viewer failed!"); + } + } + else + { + cloudViewer_->setCloudVisibility("cloudOdomLocalMap", false); + if(cloud->empty()) + UWARN("Empty cloudOdomLocalMap!"); + } + } + + if(!odom.pose().isNull()) + { + // update camera position + cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose()); + } + } + cloudViewer_->update(); + + lastOdometryProcessed_ = true; + } + + + virtual void processStatistics(const rtabmap::Statistics & stats) + { + processingStatistics_ = true; + + //============================ + // Add RGB-D clouds + //============================ + const std::map & poses = stats.poses(); + QMap clouds = cloudViewer_->getAddedClouds(); + for(std::map::const_iterator iter = poses.lower_bound(1); iter!=poses.end(); ++iter) + { + if(!iter->second.isNull()) + { + std::string cloudName = uFormat("cloud_%d", iter->first); + + // 3d point cloud + if(clouds.contains(cloudName)) + { + // Update only if the pose has changed + Transform tCloud; + cloudViewer_->getPose(cloudName, tCloud); + if(tCloud.isNull() || iter->second != tCloud) + { + if(!cloudViewer_->updateCloudPose(cloudName, iter->second)) + { + UERROR("Updating pose cloud %d failed!", iter->first); + } + } + } + else if(iter->first == stats.getLastSignatureData().id()) + { + Signature s = stats.getLastSignatureData(); + s.sensorData().uncompressData(); // make sure data is uncompressed + // Add the new cloud + pcl::PointCloud::Ptr cloud = util3d::laserScanToPointCloudI( + s.sensorData().laserScanRaw(), + s.sensorData().laserScanRaw().localTransform()); + if(cloud->size()) + { + if(!cloudViewer_->addCloud(cloudName, cloud, iter->second)) + { + UERROR("Adding cloud %d to viewer failed!", iter->first); + } + } + else + { + UWARN("Empty cloud %d!", iter->first); + } + } + + cloudViewer_->setCloudVisibility(cloudName, visibility_ == 0 || visibility_ == 1); + } + else + { + UWARN("Null pose for %d ?!?", iter->first); + } + } + + // cleanup + for(QMap::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter) + { + if(uStrContains(iter.key(), "cloud_")) + { + int id = uStr2Int(uSplitNumChar(iter.key()).back()); + if(poses.find(id) == poses.end()) + { + cloudViewer_->removeCloud(iter.key()); + } + } + } + + //============================ + // Add 3D graph (show all poses) + //============================ + cloudViewer_->removeAllGraphs(); + cloudViewer_->removeCloud("graph_nodes"); + if(poses.size()) + { + // Set graph + pcl::PointCloud::Ptr graph(new pcl::PointCloud); + pcl::PointCloud::Ptr graphNodes(new pcl::PointCloud); + for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) + { + graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z())); + } + *graphNodes = *graph; + + + // add graph + cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray); + cloudViewer_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green); + cloudViewer_->setCloudPointSize("graph_nodes", 5); + } + + odometryCorrection_ = stats.mapCorrection(); + + cloudViewer_->update(); + + processingStatistics_ = false; + } + + virtual bool handleEvent(UEvent * event) + { + if(event->getClassName().compare("RtabmapEvent") == 0) + { + RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event; + const Statistics & stats = rtabmapEvent->getStats(); + // Statistics must be processed in the Qt thread + if(this->isVisible()) + { + QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats)); + } + } + else if(event->getClassName().compare("OdometryEvent") == 0) + { + OdometryEvent * odomEvent = (OdometryEvent *)event; + // Odometry must be processed in the Qt thread + if(this->isVisible() && + lastOdometryProcessed_ && + !processingStatistics_) + { + lastOdometryProcessed_ = false; // if we receive too many odometry events! + QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent)); + } + } + return false; + } + +protected: + CloudViewer * cloudViewer_; + SensorCaptureThread * sensorCaptureThread_; + Transform lastOdomPose_; + Transform odometryCorrection_; + bool processingStatistics_; + bool lastOdometryProcessed_; + int visibility_; +}; + + +#endif /* MAPBUILDER_H_ */ diff --git a/examples/LidarMapping/main.cpp b/examples/LidarMapping/main.cpp new file mode 100644 index 0000000000..3bf0eff5e9 --- /dev/null +++ b/examples/LidarMapping/main.cpp @@ -0,0 +1,240 @@ +/* +Copyright (c) 2010-2022, Mathieu Labbe +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +// Should be first on windows to avoid "WinSock.h has already been included" error +#include "rtabmap/core/lidar/LidarVLP16.h" + +#include +#include "rtabmap/core/Rtabmap.h" +#include "rtabmap/core/RtabmapThread.h" +#include "rtabmap/core/OdometryThread.h" +#include "rtabmap/core/Graph.h" +#include "rtabmap/utilite/UEventsManager.h" +#include "rtabmap/utilite/UStl.h" +#include "rtabmap/utilite/UDirectory.h" +#include +#include +#include +#include +#include +#include + +#include "MapBuilder.h" + +void showUsage() +{ + printf("\nUsage:\n" + "rtabmap-lidar_mapping IP PORT\n" + "rtabmap-lidar_mapping PCAP_FILEPATH\n" + "\n" + "Example:" + " rtabmap-lidar_mapping 192.168.1.201 2368\n\n"); + exit(1); +} + +using namespace rtabmap; +int main(int argc, char * argv[]) +{ + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kWarning); + + std::string filepath; + std::string ip; + int port = 2368; + if(argc < 2) + { + showUsage(); + } + else if(argc == 2) + { + filepath = argv[1]; + } + else + { + ip = argv[1]; + port = uStr2Int(argv[2]); + } + + // Here is the pipeline that we will use: + // LidarVLP16 -> "SensorEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" + + // Create the Lidar sensor, it will send a SensorEvent + LidarVLP16 * lidar; + if(!ip.empty()) + { + printf("Using ip=%s port=%d\n", ip.c_str(), port); + lidar = new LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port); + } + else + { + filepath = uReplaceChar(filepath, '~', UDirectory::homeDir()); + printf("Using file=%s\n", filepath.c_str()); + lidar = new LidarVLP16(filepath); + } + lidar->setOrganized(true); //faster deskewing + + if(!lidar->init()) + { + UERROR("Lidar init failed!"); + delete lidar; + return -1; + } + + SensorCaptureThread lidarThread(lidar); + + // GUI stuff, there the handler will receive RtabmapEvent and construct the map + // We give it the lidar so the GUI can pause/resume the lidar + QApplication app(argc, argv); + MapBuilder mapBuilder(&lidarThread); + + ParametersMap params; + + float resolution = 0.05; + + // ICP parameters + params.insert(ParametersPair(Parameters::kRegStrategy(), "1")); + params.insert(ParametersPair(Parameters::kIcpFiltersEnabled(), "2")); + params.insert(ParametersPair(Parameters::kIcpPointToPlane(), "true")); + params.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), "20")); + params.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), "0")); + params.insert(ParametersPair(Parameters::kIcpIterations(), "10")); + params.insert(ParametersPair(Parameters::kIcpVoxelSize(), uNumber2Str(resolution))); + params.insert(ParametersPair(Parameters::kIcpEpsilon(), "0.001")); + params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(resolution*10.0f))); + params.insert(ParametersPair(Parameters::kIcpMaxTranslation(), "2")); + params.insert(ParametersPair(Parameters::kIcpStrategy(), "1")); + params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), "0.7")); + params.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), "0.01")); + // Uncomment if lidar never pitch/roll (on a car or wheeled robot), for hand-held mapping, keep it commented + //params.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), "0.8")); + + // Odom parameters + params.insert(ParametersPair(Parameters::kOdomStrategy(), "0")); // F2M + params.insert(ParametersPair(Parameters::kOdomScanKeyFrameThr(), "0.6")); + params.insert(ParametersPair(Parameters::kOdomF2MScanSubtractRadius(), uNumber2Str(resolution))); + params.insert(ParametersPair(Parameters::kOdomF2MScanMaxSize(), "15000")); + params.insert(ParametersPair(Parameters::kOdomGuessSmoothingDelay(), "0.3")); + params.insert(ParametersPair(Parameters::kOdomDeskewing(), "true")); + + // Create an odometry thread to process lidar events, it will send OdometryEvent. + OdometryThread odomThread(Odometry::create(params)); + + // Rtabmap params + params.insert(ParametersPair(Parameters::kRGBDProximityBySpace(), "true")); + params.insert(ParametersPair(Parameters::kRGBDProximityMaxGraphDepth(), "0")); + params.insert(ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "1")); + params.insert(ParametersPair(Parameters::kRGBDAngularUpdate(), "0.05")); + params.insert(ParametersPair(Parameters::kRGBDLinearUpdate(), "0.05")); + params.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false")); + params.insert(ParametersPair(Parameters::kMemSTMSize(), "30")); + uInsert(params, ParametersPair(Parameters::kIcpCorrespondenceRatio(), "0.2")); // overwritten + + + // Create RTAB-Map to process OdometryEvent + Rtabmap * rtabmap = new Rtabmap(); + rtabmap->init(params); + RtabmapThread rtabmapThread(rtabmap); // ownership is transfered + + // Setup handlers + odomThread.registerToEventsManager(); + rtabmapThread.registerToEventsManager(); + mapBuilder.registerToEventsManager(); + + // The RTAB-Map is subscribed by default to SensorEvent, but we want + // RTAB-Map to process OdometryEvent instead, ignoring the SensorEvent. + // We can do that by creating a "pipe" between the lidar and odometry, then + // only the odometry will receive SensorEvent from that lidar. RTAB-Map is + // also subscribed to OdometryEvent by default, so no need to create a pipe between + // odometry and RTAB-Map. + UEventsManager::createPipe(&lidarThread, &odomThread, "SensorEvent"); + + // Let's start the threads + rtabmapThread.start(); + odomThread.start(); + lidarThread.start(); + + printf("Press Tab key to switch between map and odom views (or both).\n"); + printf("Press Space key to pause.\n"); + + mapBuilder.show(); + app.exec(); // main loop + + // remove handlers + mapBuilder.unregisterFromEventsManager(); + rtabmapThread.unregisterFromEventsManager(); + odomThread.unregisterFromEventsManager(); + + // Kill all threads + lidarThread.kill(); + odomThread.join(true); + rtabmapThread.join(true); + + // Save 3D map + printf("Saving rtabmap_cloud.pcd...\n"); + std::map nodes; + std::map optimizedPoses; + std::multimap links; + rtabmap->getGraph(optimizedPoses, links, true, true, &nodes, true, true, true, true); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + for(std::map::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) + { + Signature node = nodes.find(iter->first)->second; + + // uncompress data + node.sensorData().uncompressData(); + + pcl::PointCloud::Ptr tmp = util3d::laserScanToPointCloudI(node.sensorData().laserScanRaw(), node.sensorData().laserScanRaw().localTransform()); + *cloud += *util3d::transformPointCloud(tmp, iter->second); // transform the point cloud to its pose + } + if(cloud->size()) + { + printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)cloud->size()); + cloud = util3d::voxelize(cloud, 0.01f); + + printf("Saving rtabmap_cloud.pcd... done! (%d points)\n", (int)cloud->size()); + pcl::io::savePCDFile("rtabmap_cloud.pcd", *cloud); + //pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format + } + else + { + printf("Saving rtabmap_cloud.pcd... failed! The cloud is empty.\n"); + } + + // Save trajectory + printf("Saving rtabmap_trajectory.txt ...\n"); + if(optimizedPoses.size() && graph::exportPoses("rtabmap_trajectory.txt", 0, optimizedPoses, links)) + { + printf("Saving rtabmap_trajectory.txt... done!\n"); + } + else + { + printf("Saving rtabmap_trajectory.txt... failed!\n"); + } + + rtabmap->close(false); + + return 0; +} diff --git a/examples/RGBDMapping/MapBuilder.h b/examples/RGBDMapping/MapBuilder.h index 7ae90a3547..9f788a57d6 100644 --- a/examples/RGBDMapping/MapBuilder.h +++ b/examples/RGBDMapping/MapBuilder.h @@ -46,7 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/core/OdometryEvent.h" -#include "rtabmap/core/CameraThread.h" +#include using namespace rtabmap; @@ -56,7 +56,7 @@ class MapBuilder : public QWidget, public UEventsHandler Q_OBJECT public: //Camera ownership is not transferred! - MapBuilder(CameraThread * camera = 0) : + MapBuilder(SensorCaptureThread * camera = 0) : camera_(camera), odometryCorrection_(Transform::getIdentity()), processingStatistics_(false), @@ -310,7 +310,7 @@ protected Q_SLOTS: protected: CloudViewer * cloudViewer_; - CameraThread * camera_; + SensorCaptureThread * camera_; Transform lastOdomPose_; Transform odometryCorrection_; bool processingStatistics_; diff --git a/examples/RGBDMapping/main.cpp b/examples/RGBDMapping/main.cpp index c39c56be35..28064e3bea 100644 --- a/examples/RGBDMapping/main.cpp +++ b/examples/RGBDMapping/main.cpp @@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/RtabmapThread.h" #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/OdometryThread.h" #include "rtabmap/core/Graph.h" #include "rtabmap/utilite/UEventsManager.h" @@ -39,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #ifdef RTABMAP_PYTHON #include "rtabmap/core/PythonInterface.h" @@ -80,9 +80,9 @@ int main(int argc, char * argv[]) } // Here is the pipeline that we will use: - // CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" + // CameraOpenni -> "SensorEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" - // Create the OpenNI camera, it will send a CameraEvent at the rate specified. + // Create the OpenNI camera, it will send a SensorEvent at the rate specified. // Set transform to camera so z is up, y is left and x going forward Camera * camera = 0; if(driver == 1) @@ -185,7 +185,7 @@ int main(int argc, char * argv[]) UERROR("Camera init failed!"); } - CameraThread cameraThread(camera); + SensorCaptureThread cameraThread(camera); // GUI stuff, there the handler will receive RtabmapEvent and construct the map @@ -210,19 +210,21 @@ int main(int argc, char * argv[]) rtabmapThread.registerToEventsManager(); mapBuilder.registerToEventsManager(); - // The RTAB-Map is subscribed by default to CameraEvent, but we want - // RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent. + // The RTAB-Map is subscribed by default to SensorEvent, but we want + // RTAB-Map to process OdometryEvent instead, ignoring the SensorEvent. // We can do that by creating a "pipe" between the camera and odometry, then - // only the odometry will receive CameraEvent from that camera. RTAB-Map is + // only the odometry will receive SensorEvent from that camera. RTAB-Map is // also subscribed to OdometryEvent by default, so no need to create a pipe between // odometry and RTAB-Map. - UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent"); + UEventsManager::createPipe(&cameraThread, &odomThread, "SensorEvent"); // Let's start the threads rtabmapThread.start(); odomThread.start(); cameraThread.start(); + printf("Press Space key to pause.\n"); + mapBuilder.show(); app.exec(); // main loop diff --git a/examples/WifiMapping/MapBuilder.h b/examples/WifiMapping/MapBuilder.h index 22378c9f8f..101e6035fa 100644 --- a/examples/WifiMapping/MapBuilder.h +++ b/examples/WifiMapping/MapBuilder.h @@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/core/OdometryEvent.h" -#include "rtabmap/core/CameraThread.h" +#include using namespace rtabmap; @@ -54,7 +54,7 @@ class MapBuilder : public QWidget, public UEventsHandler Q_OBJECT public: //Camera ownership is not transferred! - MapBuilder(CameraThread * camera = 0) : + MapBuilder(SensorCaptureThread * camera = 0) : camera_(camera), odometryCorrection_(Transform::getIdentity()), processingStatistics_(false), @@ -278,7 +278,7 @@ protected Q_SLOTS: protected: CloudViewer * cloudViewer_; - CameraThread * camera_; + SensorCaptureThread * camera_; Transform lastOdomPose_; Transform odometryCorrection_; bool processingStatistics_; diff --git a/examples/WifiMapping/MapBuilderWifi.h b/examples/WifiMapping/MapBuilderWifi.h index a7250382e0..dc199e5e7c 100644 --- a/examples/WifiMapping/MapBuilderWifi.h +++ b/examples/WifiMapping/MapBuilderWifi.h @@ -56,7 +56,7 @@ class MapBuilderWifi : public MapBuilder Q_OBJECT public: // Camera ownership is not transferred! - MapBuilderWifi(CameraThread * camera = 0) : + MapBuilderWifi(SensorCaptureThread * camera = 0) : MapBuilder(camera) {} diff --git a/examples/WifiMapping/main.cpp b/examples/WifiMapping/main.cpp index 847fcf20a0..82a6ebf0a3 100644 --- a/examples/WifiMapping/main.cpp +++ b/examples/WifiMapping/main.cpp @@ -26,11 +26,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/RtabmapThread.h" #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/OdometryThread.h" #include "rtabmap/utilite/UEventsManager.h" #include @@ -114,9 +114,9 @@ int main(int argc, char * argv[]) } // Here is the pipeline that we will use: - // CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" + // CameraOpenni -> "" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent" - // Create the OpenNI camera, it will send a CameraEvent at the rate specified. + // Create the OpenNI camera, it will send a at the rate specified. // Set transform to camera so z is up, y is left and x going forward Camera * camera = 0; Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0); @@ -222,7 +222,7 @@ int main(int argc, char * argv[]) showUsage(); exit(1); } - CameraThread cameraThread(camera); + SensorCaptureThread cameraThread(camera); if(mirroring) { cameraThread.setMirroringEnabled(true); @@ -253,13 +253,13 @@ int main(int argc, char * argv[]) rtabmapThread.registerToEventsManager(); mapBuilderWifi.registerToEventsManager(); - // The RTAB-Map is subscribed by default to CameraEvent, but we want - // RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent. + // The RTAB-Map is subscribed by default to , but we want + // RTAB-Map to process OdometryEvent instead, ignoring the . // We can do that by creating a "pipe" between the camera and odometry, then - // only the odometry will receive CameraEvent from that camera. RTAB-Map is + // only the odometry will receive from that camera. RTAB-Map is // also subscribed to OdometryEvent by default, so no need to create a pipe between // odometry and RTAB-Map. - UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent"); + UEventsManager::createPipe(&cameraThread, &odomThread, ""); // Let's start the threads rtabmapThread.start(); diff --git a/guilib/include/rtabmap/gui/CameraViewer.h b/guilib/include/rtabmap/gui/CameraViewer.h index 0f227f7b54..834de1bd77 100644 --- a/guilib/include/rtabmap/gui/CameraViewer.h +++ b/guilib/include/rtabmap/gui/CameraViewer.h @@ -53,6 +53,7 @@ class RTABMAP_GUI_EXPORT CameraViewer : public QDialog, public UEventsHandler QWidget * parent = 0, const ParametersMap & parameters = ParametersMap()); virtual ~CameraViewer(); + void setDecimation(int value); public Q_SLOTS: void showImage(const rtabmap::SensorData & data); diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 43988b26c0..58d4955ab4 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -37,7 +37,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/RtabmapEvent.h" #include "rtabmap/core/SensorData.h" #include "rtabmap/core/OdometryEvent.h" -#include "rtabmap/core/CameraInfo.h" #include "rtabmap/core/Optimizer.h" #include "rtabmap/core/GlobalMap.h" #include "rtabmap/gui/PreferencesDialog.h" @@ -47,9 +46,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { -class CameraThread; +class SensorCaptureThread; class OdometryThread; class IMUThread; class CloudViewer; @@ -192,6 +192,7 @@ protected Q_SLOTS: void selectDepthAIOAKD(); void selectDepthAIOAKDLite(); void selectDepthAIOAKDPro(); + void selectVLP16(); void dumpTheMemory(); void dumpThePrediction(); void sendGoal(); @@ -206,7 +207,7 @@ protected Q_SLOTS: void selectScreenCaptureFormat(bool checked); void takeScreenshot(); void updateElapsedTime(); - void processCameraInfo(const rtabmap::CameraInfo & info); + void processCameraInfo(const rtabmap::SensorCaptureInfo & info); void processOdometry(const rtabmap::OdometryEvent & odom, bool dataIgnored); void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags); void applyPrefSettings(const rtabmap::ParametersMap & parameters); @@ -241,7 +242,7 @@ protected Q_SLOTS: Q_SIGNALS: void statsReceived(const rtabmap::Statistics &); void statsProcessed(); - void cameraInfoReceived(const rtabmap::CameraInfo &); + void cameraInfoReceived(const rtabmap::SensorCaptureInfo &); void cameraInfoProcessed(); void odometryReceived(const rtabmap::OdometryEvent &, bool); void odometryProcessed(); @@ -316,11 +317,6 @@ protected Q_SLOTS: const QString & newDatabasePathOutput() const { return _newDatabasePathOutput; } virtual ParametersMap getCustomParameters() {return ParametersMap();} - virtual Camera * createCamera( - Camera ** odomSensor, - Transform & odomSensorExtrinsics, - double & odomSensorTimeOffset, - float & odomSensorScaleFactor); void postProcessing( bool refineNeighborLinks, @@ -344,7 +340,7 @@ protected Q_SLOTS: Ui_mainWindow * _ui; State _state; - rtabmap::CameraThread * _camera; + rtabmap::SensorCaptureThread * _sensorCapture; rtabmap::OdometryThread * _odomThread; rtabmap::IMUThread * _imuThread; diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index a593acc37d..0078d11dd6 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -60,6 +60,8 @@ namespace rtabmap { class Signature; class LoopClosureViewer; class Camera; +class SensorCapture; +class Lidar; class CalibrationDialog; class CreateSimpleCalibrationDialog; @@ -77,7 +79,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog kPanelAll = 15 }; // TODO, tried to change the name of PANEL_FLAGS to PanelFlags... but signals/slots errors appeared... - Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag) + Q_DECLARE_FLAGS(PANEL_FLAGS, PanelFlag); enum Src { kSrcUndef = -1, @@ -113,7 +115,10 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog kSrcImages = 201, kSrcVideo = 202, - kSrcDatabase = 300 + kSrcDatabase = 300, + + kSrcLidar = 400, + kSrcLidarVLP16 = 400, }; public: @@ -253,6 +258,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog PreferencesDialog::Src getSourceDriver() const; QString getSourceDriverStr() const; QString getSourceDevice() const; + PreferencesDialog::Src getLidarSourceDriver() const; PreferencesDialog::Src getOdomSourceDriver() const; bool isSourceDatabaseStampsUsed() const; @@ -271,6 +277,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog bool isSourceStereoDepthGenerated() const; bool isSourceStereoExposureCompensation() const; bool isSourceScanFromDepth() const; + bool isSourceScanDeskewing() const; int getSourceScanDownsampleStep() const; double getSourceScanRangeMin() const; double getSourceScanRangeMax() const; @@ -284,7 +291,8 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog QString getIMUPath() const; int getIMURate() const; Camera * createCamera(bool useRawImages = false, bool useColor = true); // return camera should be deleted if not null - Camera * createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor); // return camera should be deleted if not null + SensorCapture * createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor, double & waitTime); // return odom sensor should be deleted if not null + Lidar * createLidar(); // return lidar should be deleted if not null int getIgnoredDCComponents() const; @@ -324,6 +332,7 @@ public Q_SLOTS: void calibrate(); void calibrateSimple(); void calibrateOdomSensorExtrinsics(); + void testLidar(); private Q_SLOTS: void closeDialog ( QAbstractButton * button ); @@ -332,6 +341,7 @@ private Q_SLOTS: void loadConfigFrom(); bool saveConfigTo(); void resetConfig(); + void loadPreset(); void makeObsoleteGeneralPanel(); void makeObsoleteCloudRenderingPanel(); void makeObsoleteLoggingPanel(); @@ -389,6 +399,7 @@ private Q_SLOTS: void selectSourceSvoPath(); void selectSourceRealsense2JsonPath(); void selectSourceDepthaiBlobPath(); + void selectVlp16PcapPath(); void updateSourceGrpVisibility(); void testOdometry(); void testCamera(); @@ -417,6 +428,7 @@ private Q_SLOTS: void setupKpRoiPanel(); bool parseModel(QList & boxes, QStandardItem * parentItem, int currentLevel, int & absoluteIndex); void resetSettings(QGroupBox * groupBox); + void loadPreset(const std::string & presetHexHeader); void addParameter(const QObject * object, int value); void addParameter(const QObject * object, bool value); void addParameter(const QObject * object, double value); diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index 4782d4a76e..8eb534336f 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -177,8 +177,28 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) add_definitions(${PCL_DEFINITIONS}) +# Include presets +SET(RESOURCES + ${PROJECT_SOURCE_DIR}/data/presets/camera_tof_icp.ini + ${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp.ini +) + +foreach(arg ${RESOURCES}) + get_filename_component(filename ${arg} NAME) + string(REPLACE "." "_" output ${filename}) + set(RESOURCES_HEADERS "${RESOURCES_HEADERS}" "${CMAKE_CURRENT_BINARY_DIR}/${output}.h") + set_property(SOURCE "${CMAKE_CURRENT_BINARY_DIR}/${output}.h" PROPERTY SKIP_AUTOGEN ON) +endforeach(arg ${RESOURCES}) + +ADD_CUSTOM_COMMAND( + OUTPUT ${RESOURCES_HEADERS} + COMMAND res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES} + COMMENT "[Creating resources]" + DEPENDS ${RESOURCES} +) + # create a library from the source files -ADD_LIBRARY(rtabmap_gui ${SRC_FILES}) +ADD_LIBRARY(rtabmap_gui ${SRC_FILES} ${RESOURCES_HEADERS}) ADD_LIBRARY(rtabmap::gui ALIAS rtabmap_gui) generate_export_header(rtabmap_gui diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp index 3cec92ef89..2a9058f4e4 100644 --- a/guilib/src/CalibrationDialog.cpp +++ b/guilib/src/CalibrationDialog.cpp @@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include #include @@ -307,10 +307,10 @@ bool CalibrationDialog::handleEvent(UEvent * event) { if(!processingData_) { - if(event->getClassName().compare("CameraEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event; - if(e->getCode() == rtabmap::CameraEvent::kCodeData) + rtabmap::SensorEvent * e = (rtabmap::SensorEvent *)event; + if(e->getCode() == rtabmap::SensorEvent::kCodeData) { processingData_ = true; QMetaObject::invokeMethod(this, "processImages", diff --git a/guilib/src/CameraViewer.cpp b/guilib/src/CameraViewer.cpp index 569721ed18..2840cb10f8 100644 --- a/guilib/src/CameraViewer.cpp +++ b/guilib/src/CameraViewer.cpp @@ -25,9 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/gui/CameraViewer.h" -#include #include #include #include @@ -58,6 +58,7 @@ CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) : imageView_->setImageDepthShown(true); imageView_->setMinimumSize(320, 240); + imageView_->setVisible(false); QHBoxLayout * layout = new QHBoxLayout(); layout->setContentsMargins(0,0,0,0); layout->addWidget(imageView_,1); @@ -108,10 +109,16 @@ CameraViewer::~CameraViewer() this->unregisterFromEventsManager(); } +void CameraViewer::setDecimation(int value) +{ + decimationSpin_->setValue(value); +} + void CameraViewer::showImage(const rtabmap::SensorData & data) { processingImages_ = true; QString sizes; + imageView_->setVisible(!data.imageRaw().empty() || !data.imageRaw().empty()); if(!data.imageRaw().empty()) { imageView_->setImage(uCvMat2QImage(data.imageRaw())); @@ -199,10 +206,10 @@ bool CameraViewer::handleEvent(UEvent * event) { if(!pause_->isChecked()) { - if(event->getClassName().compare("CameraEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - CameraEvent * camEvent = (CameraEvent*)event; - if(camEvent->getCode() == CameraEvent::kCodeData) + SensorEvent * camEvent = (SensorEvent*)event; + if(camEvent->getCode() == SensorEvent::kCodeData) { if(camEvent->data().isValid()) { diff --git a/guilib/src/DataRecorder.cpp b/guilib/src/DataRecorder.cpp index 3af6637f36..af5dd0b5aa 100644 --- a/guilib/src/DataRecorder.cpp +++ b/guilib/src/DataRecorder.cpp @@ -30,8 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include -#include #include #include #include @@ -172,10 +172,10 @@ bool DataRecorder::handleEvent(UEvent * event) { if(memory_) { - if(event->getClassName().compare("CameraEvent") == 0) + if(event->getClassName().compare("SensorEvent") == 0) { - CameraEvent * camEvent = (CameraEvent*)event; - if(camEvent->getCode() == CameraEvent::kCodeData) + SensorEvent * camEvent = (SensorEvent*)event; + if(camEvent->getCode() == SensorEvent::kCodeData) { if(camEvent->data().isValid()) { diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index b5cde034d0..e2c95f1f6a 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -31,9 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/CameraRGB.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" +#include "rtabmap/core/Lidar.h" #include "rtabmap/core/IMUThread.h" -#include "rtabmap/core/CameraEvent.h" #include "rtabmap/core/DBReader.h" #include "rtabmap/core/Parameters.h" #include "rtabmap/core/ParamEvent.h" @@ -113,6 +112,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include +#include #ifdef RTABMAP_OCTOMAP #include @@ -142,7 +143,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh QMainWindow(parent), _ui(0), _state(kIdle), - _camera(0), + _sensorCapture(0), _odomThread(0), _imuThread(0), _preferencesDialog(0), @@ -468,6 +469,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh connect(_ui->actionDepthAI_oakd, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKD())); connect(_ui->actionDepthAI_oakdlite, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDLite())); connect(_ui->actionDepthAI_oakdpro, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDPro())); + connect(_ui->actionVelodyne_VLP_16, SIGNAL(triggered()), this, SLOT(selectVLP16())); _ui->actionFreenect->setEnabled(CameraFreenect::available()); _ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available()); _ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available()); @@ -566,8 +568,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh qRegisterMetaType("rtabmap::Statistics"); connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics))); - qRegisterMetaType("rtabmap::CameraInfo"); - connect(this, SIGNAL(cameraInfoReceived(rtabmap::CameraInfo)), this, SLOT(processCameraInfo(rtabmap::CameraInfo))); + qRegisterMetaType("rtabmap::SensorCaptureInfo"); + connect(this, SIGNAL(cameraInfoReceived(rtabmap::SensorCaptureInfo)), this, SLOT(processCameraInfo(rtabmap::SensorCaptureInfo))); qRegisterMetaType("rtabmap::OdometryEvent"); connect(this, SIGNAL(odometryReceived(rtabmap::OdometryEvent, bool)), this, SLOT(processOdometry(rtabmap::OdometryEvent, bool))); @@ -612,6 +614,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->statsToolBox->updateStat("Planning/Length/m", false); _ui->statsToolBox->updateStat("Camera/Time capturing/ms", false); + _ui->statsToolBox->updateStat("Camera/Time deskewing/ms", false); _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", false); _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", false); _ui->statsToolBox->updateStat("Camera/Time decimation/ms", false); @@ -641,6 +644,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", false); _ui->statsToolBox->updateStat("Odometry/VarianceLin/", false); _ui->statsToolBox->updateStat("Odometry/VarianceAng/", false); + _ui->statsToolBox->updateStat("Odometry/TimeDeskewing/ms", false); _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false); _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false); _ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", false); @@ -839,11 +843,11 @@ void MainWindow::closeEvent(QCloseEvent* event) _ui->dockWidget_odometry->close(); _ui->dockWidget_multiSessionLoc->close(); - if(_camera) + if(_sensorCapture) { UERROR("Camera must be already deleted here!"); - delete _camera; - _camera = 0; + delete _sensorCapture; + _sensorCapture = 0; if(_imuThread) { delete _imuThread; @@ -935,10 +939,10 @@ bool MainWindow::handleEvent(UEvent* anEvent) { Q_EMIT rtabmapGoalStatusEventReceived(anEvent->getCode()); } - else if(anEvent->getClassName().compare("CameraEvent") == 0) + else if(anEvent->getClassName().compare("SensorEvent") == 0) { - CameraEvent * cameraEvent = (CameraEvent*)anEvent; - if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages) + SensorEvent * sensorEvent = (SensorEvent*)anEvent; + if(sensorEvent->getCode() == SensorEvent::kCodeNoMoreImages) { if(_preferencesDialog->beepOnPause()) { @@ -948,15 +952,15 @@ bool MainWindow::handleEvent(UEvent* anEvent) } else { - Q_EMIT cameraInfoReceived(cameraEvent->info()); - if (_odomThread == 0 && (_camera->odomProvided()) && _preferencesDialog->isRGBDMode()) + Q_EMIT cameraInfoReceived(sensorEvent->info()); + if (_odomThread == 0 && (_sensorCapture->odomProvided()) && _preferencesDialog->isRGBDMode()) { OdometryInfo odomInfo; - odomInfo.reg.covariance = cameraEvent->info().odomCovariance; + odomInfo.reg.covariance = sensorEvent->info().odomCovariance; if (!_processingOdometry && !_processingStatistics) { _processingOdometry = true; // if we receive too many odometry events! - OdometryEvent tmp(cameraEvent->data(), cameraEvent->info().odomPose, odomInfo); + OdometryEvent tmp(sensorEvent->data(), sensorEvent->info().odomPose, odomInfo); Q_EMIT odometryReceived(tmp, false); } else @@ -1006,7 +1010,7 @@ bool MainWindow::handleEvent(UEvent* anEvent) return false; } -void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info) +void MainWindow::processCameraInfo(const rtabmap::SensorCaptureInfo & info) { if(_firstStamp == 0.0) { @@ -1015,6 +1019,7 @@ void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info) if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible()) { _ui->statsToolBox->updateStat("Camera/Time capturing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeCapture*1000.0f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Camera/Time deskewing/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDeskewing*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time undistort depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeUndistortDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time bilateral filtering/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeBilateralFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); @@ -1800,6 +1805,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI _ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)linVar, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)angVar), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)angVar, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/TimeDeskewing/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeDeskewing*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); if(odom.info().timeParticleFiltering>0.0f) { @@ -4863,15 +4869,15 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags) this->updateSelectSourceMenu(); _ui->label_stats_source->setText(_preferencesDialog->getSourceDriverStr()); - if(_camera) + if(_sensorCapture) { - if(dynamic_cast(_camera->camera()) != 0) + if(dynamic_cast(_sensorCapture->camera()) != 0) { - _camera->setImageRate( _preferencesDialog->isSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate()); + _sensorCapture->setFrameRate( _preferencesDialog->isSourceDatabaseStampsUsed()?-1:_preferencesDialog->getGeneralInputRate()); } else { - _camera->setImageRate(_preferencesDialog->getGeneralInputRate()); + _sensorCapture->setFrameRate(_preferencesDialog->getGeneralInputRate()); } } @@ -5293,6 +5299,7 @@ void MainWindow::updateSelectSourceMenu() _ui->actionDepthAI_oakd->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI); _ui->actionDepthAI_oakdlite->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI); _ui->actionDepthAI_oakdpro->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI); + _ui->actionVelodyne_VLP_16->setChecked(_preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcLidarVLP16); } void MainWindow::changeImgRateSetting() @@ -5710,29 +5717,6 @@ void MainWindow::editDatabase() } } -Camera * MainWindow::createCamera( - Camera ** odomSensor, - Transform & odomSensorExtrinsics, - double & odomSensorTimeOffset, - float & odomSensorScaleFactor) -{ - Camera * camera = _preferencesDialog->createCamera(); - - if(camera && - _preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef && - _preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() && - !(_preferencesDialog->getOdomSourceDriver() == PreferencesDialog::kSrcStereoRealSense2 && - _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2)) - { - UINFO("Create Odom Sensor %d (camera = %d)", - _preferencesDialog->getOdomSourceDriver(), - _preferencesDialog->getSourceDriver()); - *odomSensor = _preferencesDialog->createOdomSensor(odomSensorExtrinsics, odomSensorTimeOffset, odomSensorScaleFactor); - } - - return camera; -} - void MainWindow::startDetection() { UDEBUG(""); @@ -5810,18 +5794,19 @@ void MainWindow::startDetection() UDEBUG(""); Q_EMIT stateChanged(kStartingDetection); - if(_camera != 0) + if(_sensorCapture != 0) { QMessageBox::warning(this, tr("RTAB-Map"), tr("A camera is running, stop it first.")); - UWARN("_camera is not null... it must be stopped first"); + UWARN("_sensorCapture is not null... it must be stopped first"); Q_EMIT stateChanged(kInitialized); return; } // Adjust pre-requirements - if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef) + if(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcUndef && + _preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcUndef) { QMessageBox::warning(this, tr("RTAB-Map"), @@ -5834,34 +5819,75 @@ void MainWindow::startDetection() double poseTimeOffset = 0.0; float scaleFactor = 0.0f; + double waitTime = 0.1; Transform extrinsics; - Camera * odomSensor = 0; - Camera * camera = this->createCamera(&odomSensor, extrinsics, poseTimeOffset, scaleFactor); - if(!camera) + SensorCapture * odomSensor = 0; + Camera * camera = 0; + Lidar * lidar = 0; + + if(_preferencesDialog->getLidarSourceDriver() != PreferencesDialog::kSrcUndef) { - Q_EMIT stateChanged(kInitialized); - return; + lidar = _preferencesDialog->createLidar(); + if(!lidar) + { + Q_EMIT stateChanged(kInitialized); + return; + } } - if(odomSensor) + if(!lidar || _preferencesDialog->getSourceDriver() != PreferencesDialog::kSrcUndef) { - _camera = new CameraThread(camera, odomSensor, extrinsics, poseTimeOffset, scaleFactor, _preferencesDialog->isOdomSensorAsGt(), parameters); + camera = _preferencesDialog->createCamera(); + if(!camera) + { + delete lidar; + Q_EMIT stateChanged(kInitialized); + return; + } } - else + + if(_preferencesDialog->getOdomSourceDriver() != PreferencesDialog::kSrcUndef) { - _camera = new CameraThread(camera, _preferencesDialog->isOdomSensorAsGt(), parameters); + if(camera == 0 || + (_preferencesDialog->getOdomSourceDriver() != _preferencesDialog->getSourceDriver() && + !(_preferencesDialog->getOdomSourceDriver() == PreferencesDialog::kSrcStereoRealSense2 && + _preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2))) + { + UINFO("Create Odom Sensor %d (camera = %d)", + _preferencesDialog->getOdomSourceDriver(), + _preferencesDialog->getSourceDriver()); + odomSensor = _preferencesDialog->createOdomSensor(extrinsics, poseTimeOffset, scaleFactor, waitTime); + if(!odomSensor) + { + delete camera; + delete lidar; + Q_EMIT stateChanged(kInitialized); + return; + } + } + else if(camera->odomProvided()) + { + UINFO("The camera is also the odometry sensor (camera=%d odom=%d).", + _preferencesDialog->getSourceDriver(), + _preferencesDialog->getOdomSourceDriver()); + odomSensor = camera; + } } - _camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); - _camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); - _camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation()); - _camera->setHistogramMethod(_preferencesDialog->getSourceHistogramMethod()); + + _sensorCapture = new SensorCaptureThread(lidar, camera, odomSensor, extrinsics, poseTimeOffset, scaleFactor, waitTime, parameters); + + _sensorCapture->setOdomAsGroundTruth(_preferencesDialog->isOdomSensorAsGt()); + _sensorCapture->setMirroringEnabled(_preferencesDialog->isSourceMirroring()); + _sensorCapture->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly()); + _sensorCapture->setImageDecimation(_preferencesDialog->getSourceImageDecimation()); + _sensorCapture->setHistogramMethod(_preferencesDialog->getSourceHistogramMethod()); if(_preferencesDialog->isSourceFeatureDetection()) { - _camera->enableFeatureDetection(parameters); + _sensorCapture->enableFeatureDetection(parameters); } - _camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated()); - _camera->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation()); - _camera->setScanParameters( + _sensorCapture->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated()); + _sensorCapture->setStereoExposureCompensation(_preferencesDialog->isSourceStereoExposureCompensation()); + _sensorCapture->setScanParameters( _preferencesDialog->isSourceScanFromDepth(), _preferencesDialog->getSourceScanDownsampleStep(), _preferencesDialog->getSourceScanRangeMin(), @@ -5869,32 +5895,33 @@ void MainWindow::startDetection() _preferencesDialog->getSourceScanVoxelSize(), _preferencesDialog->getSourceScanNormalsK(), _preferencesDialog->getSourceScanNormalsRadius(), - (float)_preferencesDialog->getSourceScanForceGroundNormalsUp()); + (float)_preferencesDialog->getSourceScanForceGroundNormalsUp(), + _preferencesDialog->isSourceScanDeskewing()); if(_preferencesDialog->getIMUFilteringStrategy()>0 && dynamic_cast(camera) == 0) { - _camera->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion()); + _sensorCapture->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion()); } if(_preferencesDialog->isDepthFilteringAvailable()) { if(_preferencesDialog->isBilateralFiltering()) { - _camera->enableBilateralFiltering( + _sensorCapture->enableBilateralFiltering( _preferencesDialog->getBilateralSigmaS(), _preferencesDialog->getBilateralSigmaR()); } - _camera->setDistortionModel(_preferencesDialog->getSourceDistortionModel().toStdString()); + _sensorCapture->setDistortionModel(_preferencesDialog->getSourceDistortionModel().toStdString()); } //Create odometry thread if rgbd slam if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str())) { // Require calibrated camera - if(!camera->isCalibrated()) + if(camera && !camera->isCalibrated()) { UWARN("Camera is not calibrated!"); Q_EMIT stateChanged(kInitialized); - delete _camera; - _camera = 0; + delete _sensorCapture; + _sensorCapture = 0; int button = QMessageBox::question(this, tr("Camera is not calibrated!"), @@ -5922,7 +5949,7 @@ void MainWindow::startDetection() _imuThread = 0; } - if((!_camera->odomProvided() || _preferencesDialog->isOdomSensorAsGt()) && !_preferencesDialog->isOdomDisabled()) + if(!_sensorCapture->odomProvided() && !_preferencesDialog->isOdomDisabled()) { ParametersMap odomParameters = parameters; if(_preferencesDialog->getOdomRegistrationApproach() < 3) @@ -5958,8 +5985,8 @@ void MainWindow::startDetection() { QMessageBox::warning(this, tr("Source IMU Path"), tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok); - delete _camera; - _camera = 0; + delete _sensorCapture; + _sensorCapture = 0; delete _imuThread; _imuThread = 0; return; @@ -5969,8 +5996,8 @@ void MainWindow::startDetection() _odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize()); UEventsManager::addHandler(_odomThread); - UEventsManager::createPipe(_camera, _odomThread, "CameraEvent"); - UEventsManager::createPipe(_camera, this, "CameraEvent"); + UEventsManager::createPipe(_sensorCapture, _odomThread, "SensorEvent"); + UEventsManager::createPipe(_sensorCapture, this, "SensorEvent"); if(_imuThread) { UEventsManager::createPipe(_imuThread, _odomThread, "IMUEvent"); @@ -5980,9 +6007,9 @@ void MainWindow::startDetection() } } - if(_dataRecorder && _camera && _odomThread) + if(_dataRecorder && _sensorCapture && _odomThread) { - UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent"); + UEventsManager::createPipe(_sensorCapture, _dataRecorder, "SensorEvent"); } _lastOdomPose.setNull(); @@ -6036,7 +6063,7 @@ void MainWindow::startDetection() // Could not be in the main thread here! (see handleEvents()) void MainWindow::pauseDetection() { - if(_camera) + if(_sensorCapture) { if(_state == kPaused && (QApplication::keyboardModifiers() & Qt::ShiftModifier)) { @@ -6070,13 +6097,13 @@ void MainWindow::pauseDetection() void MainWindow::stopDetection() { - if(!_camera && !_odomThread) + if(!_sensorCapture && !_odomThread) { return; } if(_state == kDetecting && - (_camera && _camera->isRunning()) ) + (_sensorCapture && _sensorCapture->isRunning()) ) { QMessageBox::StandardButton button = QMessageBox::question(this, tr("Stopping process..."), tr("Are you sure you want to stop the process?"), QMessageBox::Yes|QMessageBox::No, QMessageBox::No); @@ -6093,9 +6120,9 @@ void MainWindow::stopDetection() _imuThread->join(true); } - if(_camera) + if(_sensorCapture) { - _camera->join(true); + _sensorCapture->join(true); } if(_odomThread) @@ -6110,10 +6137,10 @@ void MainWindow::stopDetection() delete _imuThread; _imuThread = 0; } - if(_camera) + if(_sensorCapture) { - delete _camera; - _camera = 0; + delete _sensorCapture; + _sensorCapture = 0; } if(_odomThread) { @@ -7201,6 +7228,11 @@ void MainWindow::selectDepthAIOAKDPro() _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoDepthAI, 2); // variant 2=IMU+color } +void MainWindow::selectVLP16() +{ + _preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcLidarVLP16); +} + void MainWindow::dumpTheMemory() { this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory)); @@ -8311,9 +8343,9 @@ void MainWindow::dataRecorder() this->connect(_dataRecorder, SIGNAL(destroyed(QObject*)), this, SLOT(dataRecorderDestroyed())); _dataRecorder->show(); _dataRecorder->registerToEventsManager(); - if(_camera) + if(_sensorCapture) { - UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent"); + UEventsManager::createPipe(_sensorCapture, _dataRecorder, "SensorEvent"); } _ui->actionData_recorder->setEnabled(false); } @@ -8580,9 +8612,9 @@ void MainWindow::changeState(MainWindow::State newState) _databaseUpdated = true; // if a new database is used, it won't be empty anymore... - if(_camera) + if(_sensorCapture) { - _camera->start(); + _sensorCapture->start(); if(_imuThread) { _imuThread->start(); @@ -8616,9 +8648,9 @@ void MainWindow::changeState(MainWindow::State newState) _elapsedTime->start(); _oneSecondTimer->start(); - if(_camera) + if(_sensorCapture) { - _camera->start(); + _sensorCapture->start(); if(_imuThread) { _imuThread->start(); @@ -8654,13 +8686,13 @@ void MainWindow::changeState(MainWindow::State newState) _oneSecondTimer->stop(); // kill sensors - if(_camera) + if(_sensorCapture) { if(_imuThread) { _imuThread->join(true); } - _camera->join(true); + _sensorCapture->join(true); } } break; diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 07f4d5215a..c802459f85 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -26,6 +26,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +// Should be first on windows to avoid "WinSock.h has already been included" error +#include "rtabmap/core/lidar/LidarVLP16.h" + #include "rtabmap/gui/PreferencesDialog.h" #include "rtabmap/gui/DatabaseViewer.h" #include "rtabmap/gui/OdometryViewer.h" @@ -56,9 +59,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Odometry.h" #include "rtabmap/core/OdometryThread.h" #include "rtabmap/core/CameraRGBD.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/CameraRGB.h" #include "rtabmap/core/CameraStereo.h" +#include "rtabmap/core/IMUFilter.h" #include "rtabmap/core/IMUThread.h" #include "rtabmap/core/Memory.h" #include "rtabmap/core/VWDictionary.h" @@ -84,7 +87,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/utilite/UPlot.h" +// Presets +#include "camera_tof_icp_ini.h" +#include "lidar3d_icp_ini.h" + #include +#include #if CV_MAJOR_VERSION < 3 #ifdef HAVE_OPENCV_GPU #include @@ -445,6 +453,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); + connect(_ui->comboBox_odom_sensor, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); + connect(_ui->comboBox_lidar_src, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility())); + connect(_ui->checkBox_source_scanFromDepth, SIGNAL(stateChanged(int)), this, SLOT(updateSourceGrpVisibility())); this->resetSettings(_ui->groupBox_source0); _ui->predictionPlot->showLegend(false); @@ -459,9 +470,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->pushButton_loadConfig, SIGNAL(clicked()), this, SLOT(loadConfigFrom())); connect(_ui->pushButton_saveConfig, SIGNAL(clicked()), this, SLOT(saveConfigTo())); connect(_ui->pushButton_resetConfig, SIGNAL(clicked()), this, SLOT(resetConfig())); + connect(_ui->pushButton_presets_camera_tof_icp, SIGNAL(clicked()), this, SLOT(loadPreset())); + connect(_ui->pushButton_presets_lidar_3d_icp, SIGNAL(clicked()), this, SLOT(loadPreset())); connect(_ui->radioButton_basic, SIGNAL(toggled(bool)), this, SLOT(setupTreeView())); connect(_ui->pushButton_testOdometry, SIGNAL(clicked()), this, SLOT(testOdometry())); connect(_ui->pushButton_test_camera, SIGNAL(clicked()), this, SLOT(testCamera())); + connect(_ui->pushButton_test_lidar, SIGNAL(clicked()), this, SLOT(testLidar())); // General panel connect(_ui->general_checkBox_imagesKept, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel())); @@ -862,6 +876,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->lineEdit_odomSourceDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_odom_sensor_time_offset, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_odom_sensor_scale_factor, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_odom_sensor_wait_time, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_odom_sensor_use_as_gt, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -870,6 +885,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_imuFilter_baseFrameConversion, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_publishInterIMU, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_lidar_src, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->comboBox_lidar_src, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_lidar_src, SLOT(setCurrentIndex(int))); + _ui->stackedWidget_lidar_src->setCurrentIndex(_ui->comboBox_lidar_src->currentIndex()); + connect(_ui->checkBox_source_scanDeskewing, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkBox_source_scanFromDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_source_scanDownsampleStep, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_source_scanRangeMin, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); @@ -879,6 +898,17 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->doubleSpinBox_source_scanNormalsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_source_scanNormalsForceGroundUp, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->lineEdit_lidar_local_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->toolButton_vlp16_pcap_path, SIGNAL(clicked()), this, SLOT(selectVlp16PcapPath())); + connect(_ui->lineEdit_vlp16_pcap_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_ip1, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_ip2, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_ip2, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_ip4, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_vlp16_port, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_vlp16_organized, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_vlp16_hostTime, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkBox_vlp16_stamp_last, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); //Rtabmap basic connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double))); @@ -1239,6 +1269,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str()); _ui->loopClosure_icpRangeMin->setObjectName(Parameters::kIcpRangeMin().c_str()); _ui->loopClosure_icpRangeMax->setObjectName(Parameters::kIcpRangeMax().c_str()); + _ui->loopClosure_icpFiltersEnabled->setObjectName(Parameters::kIcpFiltersEnabled().c_str()); _ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kIcpMaxCorrespondenceDistance().c_str()); _ui->loopClosure_icpReciprocalCorrespondences->setObjectName(Parameters::kIcpReciprocalCorrespondences().c_str()); _ui->loopClosure_icpIterations->setObjectName(Parameters::kIcpIterations().c_str()); @@ -1323,6 +1354,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->odom_guess_smoothing_delay->setObjectName(Parameters::kOdomGuessSmoothingDelay().c_str()); _ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str()); _ui->odom_alignWithGround->setObjectName(Parameters::kOdomAlignWithGround().c_str()); + _ui->odom_lidar_deskewing->setObjectName(Parameters::kOdomDeskewing().c_str()); //Odometry Frame to Map _ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str()); @@ -2208,6 +2240,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->lineEdit_odomSourceDevice->setText(""); _ui->doubleSpinBox_odom_sensor_time_offset->setValue(0.0); _ui->doubleSpinBox_odom_sensor_scale_factor->setValue(1); + _ui->doubleSpinBox_odom_sensor_wait_time->setValue(100); _ui->checkBox_odom_sensor_use_as_gt->setChecked(false); _ui->comboBox_imuFilter_strategy->setCurrentIndex(2); @@ -2220,6 +2253,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_imuFilter_baseFrameConversion->setChecked(true); _ui->checkbox_publishInterIMU->setChecked(false); + _ui->comboBox_lidar_src->setCurrentIndex(0); + _ui->checkBox_source_scanDeskewing->setChecked(false); _ui->checkBox_source_scanFromDepth->setChecked(false); _ui->spinBox_source_scanDownsampleStep->setValue(1); _ui->doubleSpinBox_source_scanRangeMin->setValue(0); @@ -2229,6 +2264,17 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->doubleSpinBox_source_scanNormalsRadius->setValue(0.0); _ui->doubleSpinBox_source_scanNormalsForceGroundUp->setValue(0); + _ui->lineEdit_lidar_local_transform->setText("0 0 0 0 0 0"); + _ui->lineEdit_vlp16_pcap_path->clear(); + _ui->spinBox_vlp16_ip1->setValue(192); + _ui->spinBox_vlp16_ip2->setValue(168); + _ui->spinBox_vlp16_ip3->setValue(1); + _ui->spinBox_vlp16_ip4->setValue(201); + _ui->spinBox_vlp16_port->setValue(2368); + _ui->checkBox_vlp16_organized->setChecked(false); + _ui->checkBox_vlp16_hostTime->setChecked(true); + _ui->checkBox_vlp16_stamp_last->setChecked(true); + _ui->groupBox_depthFromScan->setChecked(false); _ui->groupBox_depthFromScan_fillHoles->setChecked(true); _ui->radioButton_depthFromScan_vertical->setChecked(true); @@ -2715,6 +2761,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->lineEdit_odomSourceDevice->setText(settings.value("odom_sensor_device", _ui->lineEdit_odomSourceDevice->text()).toString()); _ui->doubleSpinBox_odom_sensor_time_offset->setValue(settings.value("odom_sensor_offset_time", _ui->doubleSpinBox_odom_sensor_time_offset->value()).toDouble()); _ui->doubleSpinBox_odom_sensor_scale_factor->setValue(settings.value("odom_sensor_scale_factor", _ui->doubleSpinBox_odom_sensor_scale_factor->value()).toDouble()); + _ui->doubleSpinBox_odom_sensor_wait_time->setValue(settings.value("odom_sensor_wait_time", _ui->doubleSpinBox_odom_sensor_wait_time->value()).toDouble()); _ui->checkBox_odom_sensor_use_as_gt->setChecked(settings.value("odom_sensor_odom_as_gt", _ui->checkBox_odom_sensor_use_as_gt->isChecked()).toBool()); settings.endGroup(); // OdomSensor @@ -2740,6 +2787,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.endGroup();//IMU settings.beginGroup("Scan"); + _ui->comboBox_lidar_src->setCurrentIndex(settings.value("source", _ui->comboBox_lidar_src->currentIndex()).toInt()); + _ui->checkBox_source_scanDeskewing->setChecked(settings.value("deskewing", _ui->checkBox_source_scanDeskewing->isChecked()).toBool()); _ui->checkBox_source_scanFromDepth->setChecked(settings.value("fromDepth", _ui->checkBox_source_scanFromDepth->isChecked()).toBool()); _ui->spinBox_source_scanDownsampleStep->setValue(settings.value("downsampleStep", _ui->spinBox_source_scanDownsampleStep->value()).toInt()); _ui->doubleSpinBox_source_scanRangeMin->setValue(settings.value("rangeMin", _ui->doubleSpinBox_source_scanRangeMin->value()).toDouble()); @@ -2774,6 +2823,23 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.endGroup(); // Camera + settings.beginGroup("Lidar"); + + settings.beginGroup("VLP16"); + _ui->lineEdit_lidar_local_transform->setText(settings.value("localTransform",_ui->lineEdit_lidar_local_transform->text()).toString()); + _ui->lineEdit_vlp16_pcap_path->setText(settings.value("pcapPath",_ui->lineEdit_vlp16_pcap_path->text()).toString()); + _ui->spinBox_vlp16_ip1->setValue(settings.value("ip1", _ui->spinBox_vlp16_ip1->value()).toInt()); + _ui->spinBox_vlp16_ip2->setValue(settings.value("ip2", _ui->spinBox_vlp16_ip2->value()).toInt()); + _ui->spinBox_vlp16_ip3->setValue(settings.value("ip3", _ui->spinBox_vlp16_ip3->value()).toInt()); + _ui->spinBox_vlp16_ip4->setValue(settings.value("ip4", _ui->spinBox_vlp16_ip4->value()).toInt()); + _ui->spinBox_vlp16_port->setValue(settings.value("port", _ui->spinBox_vlp16_port->value()).toInt()); + _ui->checkBox_vlp16_organized->setChecked(settings.value("organized", _ui->checkBox_vlp16_organized->isChecked()).toBool()); + _ui->checkBox_vlp16_hostTime->setChecked(settings.value("hostTime", _ui->checkBox_vlp16_hostTime->isChecked()).toBool()); + _ui->checkBox_vlp16_stamp_last->setChecked(settings.value("stampLast", _ui->checkBox_vlp16_stamp_last->isChecked()).toBool()); + settings.endGroup(); // VLP16 + + settings.endGroup(); // Lidar + _calibrationDialog->loadSettings(settings, "CalibrationDialog"); } @@ -2898,6 +2964,45 @@ void PreferencesDialog::resetConfig() } } +void PreferencesDialog::loadPreset(const std::string & presetHexHeader) +{ + ParametersMap parameters = Parameters::getDefaultParameters(); + if(!presetHexHeader.empty()) + { + Parameters::readINIStr(uHex2Str(presetHexHeader), parameters); + } + + // Reset 3D rendering panel + this->resetSettings(1); + // Update parameters + parameters.erase(Parameters::kRtabmapWorkingDirectory()); + for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter) + { + this->setParameter(iter->first, iter->second); + } +} + +void PreferencesDialog::loadPreset() +{ + if(sender() == _ui->pushButton_presets_camera_tof_icp) + { + loadPreset(CAMERA_TOF_ICP_INI); + } + else if(sender() == _ui->pushButton_presets_lidar_3d_icp) + { + loadPreset(LIDAR3D_ICP_INI); + } + else + { + UERROR("Unknown sender!"); + return; + } + QMessageBox::information(this, + tr("Preset"), + tr("Loaded \"%1\" preset!").arg(((QPushButton*)sender())->text()), + QMessageBox::Ok); +} + void PreferencesDialog::writeSettings(const QString & filePath) { writeGuiSettings(filePath); @@ -3254,6 +3359,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("odom_sensor_device", _ui->lineEdit_odomSourceDevice->text()); settings.setValue("odom_sensor_offset_time", _ui->doubleSpinBox_odom_sensor_time_offset->value()); settings.setValue("odom_sensor_scale_factor", _ui->doubleSpinBox_odom_sensor_scale_factor->value()); + settings.setValue("odom_sensor_wait_time", _ui->doubleSpinBox_odom_sensor_wait_time->value()); settings.setValue("odom_sensor_odom_as_gt", _ui->checkBox_odom_sensor_use_as_gt->isChecked()); settings.endGroup(); // OdomSensor @@ -3279,6 +3385,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup();//IMU settings.beginGroup("Scan"); + settings.setValue("source", _ui->comboBox_lidar_src->currentIndex()); + settings.setValue("deskewing", _ui->checkBox_source_scanDeskewing->isChecked()); settings.setValue("fromDepth", _ui->checkBox_source_scanFromDepth->isChecked()); settings.setValue("downsampleStep", _ui->spinBox_source_scanDownsampleStep->value()); settings.setValue("rangeMin", _ui->doubleSpinBox_source_scanRangeMin->value()); @@ -3313,6 +3421,23 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup(); // Camera + settings.beginGroup("Lidar"); + + settings.beginGroup("VLP16"); + settings.setValue("localTransform",_ui->lineEdit_lidar_local_transform->text()); + settings.setValue("pcapPath",_ui->lineEdit_vlp16_pcap_path->text()); + settings.setValue("ip1", _ui->spinBox_vlp16_ip1->value()); + settings.setValue("ip2", _ui->spinBox_vlp16_ip2->value()); + settings.setValue("ip3", _ui->spinBox_vlp16_ip3->value()); + settings.setValue("ip4", _ui->spinBox_vlp16_ip4->value()); + settings.setValue("port", _ui->spinBox_vlp16_port->value()); + settings.setValue("organized", _ui->checkBox_vlp16_organized->isChecked()); + settings.setValue("hostTime", _ui->checkBox_vlp16_hostTime->isChecked()); + settings.setValue("stampLast", _ui->checkBox_vlp16_stamp_last->isChecked()); + settings.endGroup(); // VLP16 + + settings.endGroup(); // Lidar + _calibrationDialog->saveSettings(settings, "CalibrationDialog"); } @@ -4049,6 +4174,9 @@ void PreferencesDialog::updateParameters(const ParametersMap & parameters, bool void PreferencesDialog::selectSourceDriver(Src src, int variant) { + Src previousCameraSrc = getSourceDriver(); + Src previousLidarSrc = getLidarSourceDriver(); + if(_ui->comboBox_imuFilter_strategy->currentIndex()==0) { _ui->comboBox_imuFilter_strategy->setCurrentIndex(2); @@ -4117,10 +4245,33 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant) _ui->comboBox_sourceType->setCurrentIndex(2); _ui->source_comboBox_image_type->setCurrentIndex(src - kSrcRGB); } - else if(src >= kSrcDatabase) + else if(src == kSrcDatabase) { _ui->comboBox_sourceType->setCurrentIndex(3); } + else if(src >= kSrcLidar) + { + _ui->comboBox_lidar_src->setCurrentIndex(kSrcLidarVLP16 - kSrcLidar + 1); + } + + if(previousCameraSrc == kSrcUndef && src < kSrcDatabase && + QMessageBox::question(this, tr("Camera Source..."), + tr("Do you want to use default camera settings?"), + QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes) + { + loadPreset(""); + _ui->comboBox_lidar_src->setCurrentIndex(0); // Set Lidar to None; + _ui->comboBox_odom_sensor->setCurrentIndex(0); // Set odom sensor to None + } + else if(previousLidarSrc== kSrcUndef && src >= kSrcLidar && + QMessageBox::question(this, tr("LiDAR Source..."), + tr("Do you want to use \"LiDAR 3D ICP\" preset?"), + QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes) + { + loadPreset(LIDAR3D_ICP_INI); + _ui->comboBox_sourceType->setCurrentIndex(4); // Set camera to None; + _ui->comboBox_odom_sensor->setCurrentIndex(0); // Set odom sensor to No + } if(validateForm()) { @@ -4571,6 +4722,20 @@ void PreferencesDialog::selectSourceDepthaiBlobPath() } } +void PreferencesDialog::selectVlp16PcapPath() +{ + QString dir = _ui->lineEdit_vlp16_pcap_path->text(); + if (dir.isEmpty()) + { + dir = getWorkingDirectory(); + } + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Velodyne recording (*.pcap)")); + if (!path.isEmpty()) + { + _ui->lineEdit_vlp16_pcap_path->setText(path); + } +} + void PreferencesDialog::setParameter(const std::string & key, const std::string & value) { UDEBUG("%s=%s", key.c_str(), value.c_str()); @@ -5432,14 +5597,14 @@ void PreferencesDialog::changePyDetectorPath() void PreferencesDialog::updateSourceGrpVisibility() { + _ui->stackedWidget_src->setVisible(_ui->comboBox_sourceType->currentIndex() != 4); // Not Camera None + _ui->frame_camera_sensor->setVisible(_ui->comboBox_sourceType->currentIndex() != 4); // Not Camera None + _ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0); _ui->groupBox_sourceStereo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1); _ui->groupBox_sourceRGB->setVisible(_ui->comboBox_sourceType->currentIndex() == 2); _ui->groupBox_sourceDatabase->setVisible(_ui->comboBox_sourceType->currentIndex() == 3); - _ui->checkBox_source_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3); - _ui->label_source_scanFromDepth->setVisible(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3); - _ui->stackedWidget_rgbd->setVisible(_ui->comboBox_sourceType->currentIndex() == 0 && (_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcRGBD || _ui->comboBox_cameraRGBD->currentIndex() == kSrcFreenect2-kSrcRGBD || @@ -5492,6 +5657,30 @@ void PreferencesDialog::updateSourceGrpVisibility() _ui->comboBox_sourceType->currentIndex() == 3); // Database _ui->groupBox_depthImageFiltering->setVisible(_ui->groupBox_depthImageFiltering->isEnabled()); + _ui->groupBox_depthFromScan->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB); + + // Odom Sensor Group + _ui->frame_visual_odometry_sensor->setVisible(getOdomSourceDriver() != kSrcUndef); // Not Lidar None + _ui->groupBox_odom_sensor->setVisible(_ui->comboBox_sourceType->currentIndex() != 3); // Don't show when database is selected + + // Lidar Sensor Group + _ui->comboBox_lidar_src->setEnabled(_ui->comboBox_sourceType->currentIndex() != 3); // Disable if database input + if(!_ui->comboBox_lidar_src->isEnabled() && _ui->comboBox_lidar_src->currentIndex() != 0) + { + _ui->comboBox_lidar_src->setCurrentIndex(0); // Set to none + } + _ui->checkBox_source_scanFromDepth->setEnabled(_ui->comboBox_sourceType->currentIndex() <= 1 || _ui->comboBox_sourceType->currentIndex() == 3); + _ui->label_source_scanFromDepth->setEnabled(_ui->checkBox_source_scanFromDepth->isEnabled()); + if(!_ui->checkBox_source_scanFromDepth->isEnabled()) + { + _ui->checkBox_source_scanFromDepth->setChecked(false); + } + _ui->stackedWidget_lidar_src->setVisible(_ui->comboBox_lidar_src->currentIndex() > 0); + _ui->groupBox_vlp16->setVisible(_ui->comboBox_lidar_src->currentIndex()-1 == kSrcLidarVLP16-kSrcLidar); + _ui->frame_lidar_sensor->setVisible(_ui->comboBox_lidar_src->currentIndex() > 0 || _ui->checkBox_source_scanFromDepth->isChecked()); // Not Lidar None or database input + _ui->pushButton_test_lidar->setEnabled(_ui->comboBox_lidar_src->currentIndex() > 0); + + // IMU group _ui->groupBox_imuFiltering->setEnabled( (_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) || (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) || @@ -5504,14 +5693,11 @@ void PreferencesDialog::updateSourceGrpVisibility() (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo) || // MYNT EYE S (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZedOC - kSrcStereo) || (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoDepthAI - kSrcStereo)); + _ui->frame_imu_filtering->setVisible(getIMUFilteringStrategy() > 0); // Not None _ui->stackedWidget_imuFilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() > 0); _ui->groupBox_madgwickfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 1); _ui->groupBox_complementaryfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 2); _ui->groupBox_imuFiltering->setVisible(_ui->groupBox_imuFiltering->isEnabled()); - - //_ui->groupBox_scan->setVisible(_ui->comboBox_sourceType->currentIndex() != 3); - - _ui->groupBox_depthFromScan->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB); } /*** GETTERS ***/ @@ -6048,6 +6234,15 @@ PreferencesDialog::Src PreferencesDialog::getOdomSourceDriver() const return kSrcUndef; } +PreferencesDialog::Src PreferencesDialog::getLidarSourceDriver() const +{ + if(_ui->comboBox_lidar_src->currentIndex() == 0) + { + return kSrcUndef; + } + return (PreferencesDialog::Src)(_ui->comboBox_lidar_src->currentIndex()-1 + kSrcLidar); +} + Transform PreferencesDialog::getSourceLocalTransform() const { Transform t = Transform::fromString(_ui->lineEdit_sourceLocalTransform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString()); @@ -6146,6 +6341,10 @@ bool PreferencesDialog::isSourceScanFromDepth() const { return _ui->checkBox_source_scanFromDepth->isChecked(); } +bool PreferencesDialog::isSourceScanDeskewing() const +{ + return _ui->checkBox_source_scanDeskewing->isChecked(); +} int PreferencesDialog::getSourceScanDownsampleStep() const { return _ui->spinBox_source_scanDownsampleStep->value(); @@ -6338,11 +6537,14 @@ Camera * PreferencesDialog::createCamera( device.isEmpty()&&driver == kSrcStereoRealSense2?"T265":device.toStdString(), this->getGeneralInputRate(), this->getSourceLocalTransform()); - ((CameraRealSense2*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); + camera->setInterIMUPublishing( + _ui->checkbox_publishInterIMU->isChecked(), + _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? + IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); if(driver == kSrcStereoRealSense2) { ((CameraRealSense2*)camera)->setImagesRectified((_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages); - ((CameraRealSense2*)camera)->setOdomProvided(_ui->comboBox_odom_sensor->currentIndex() == 1 || odomOnly, odomOnly, odomSensorExtrinsicsCalib); + ((CameraRealSense2*)camera)->setOdomProvided(getOdomSourceDriver() == kSrcStereoRealSense2 || odomOnly, odomOnly, odomSensorExtrinsicsCalib); } else { @@ -6351,7 +6553,7 @@ Camera * PreferencesDialog::createCamera( ((CameraRealSense2*)camera)->setResolution(_ui->spinBox_rs2_width->value(), _ui->spinBox_rs2_height->value(), _ui->spinBox_rs2_rate->value()); ((CameraRealSense2*)camera)->setDepthResolution(_ui->spinBox_rs2_width_depth->value(), _ui->spinBox_rs2_height_depth->value(), _ui->spinBox_rs2_rate_depth->value()); ((CameraRealSense2*)camera)->setGlobalTimeSync(_ui->checkbox_rs2_globalTimeStync->isChecked()); - ((CameraRealSense2*)camera)->setDualMode(_ui->comboBox_odom_sensor->currentIndex()==1, Transform::fromString(_ui->lineEdit_odom_sensor_extrinsics->text().toStdString())); + ((CameraRealSense2*)camera)->setDualMode(getOdomSourceDriver() == kSrcStereoRealSense2, Transform::fromString(_ui->lineEdit_odom_sensor_extrinsics->text().toStdString())); ((CameraRealSense2*)camera)->setJsonConfig(_ui->lineEdit_rs2_jsonFile->text().toStdString()); } } @@ -6524,7 +6726,7 @@ Camera * PreferencesDialog::createCamera( _ui->comboBox_stereoZed_quality->currentIndex(), _ui->comboBox_stereoZed_sensingMode->currentIndex(), _ui->spinBox_stereoZed_confidenceThr->value(), - _ui->comboBox_odom_sensor->currentIndex() == 2, + getOdomSourceDriver() == kSrcStereoZed, this->getGeneralInputRate(), this->getSourceLocalTransform(), _ui->checkbox_stereoZed_selfCalibration->isChecked(), @@ -6540,14 +6742,17 @@ Camera * PreferencesDialog::createCamera( _ui->comboBox_stereoZed_quality->currentIndex()==0&&odomOnly?1:_ui->comboBox_stereoZed_quality->currentIndex(), _ui->comboBox_stereoZed_sensingMode->currentIndex(), _ui->spinBox_stereoZed_confidenceThr->value(), - _ui->comboBox_odom_sensor->currentIndex() == 2 || odomOnly, + getOdomSourceDriver() == kSrcStereoZed || odomOnly, this->getGeneralInputRate(), this->getSourceLocalTransform(), _ui->checkbox_stereoZed_selfCalibration->isChecked(), _ui->loopClosure_bowForce2D->isChecked(), _ui->spinBox_stereoZed_texturenessConfidenceThr->value()); } - ((CameraStereoZed*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); + camera->setInterIMUPublishing( + _ui->checkbox_publishInterIMU->isChecked(), + _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? + IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); } else if (driver == kSrcStereoZedOC) { @@ -6713,7 +6918,7 @@ Camera * PreferencesDialog::createCamera( return camera; } -Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor) +SensorCapture * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & timeOffset, float & scaleFactor, double & waitTime) { Src driver = getOdomSourceDriver(); if(driver != kSrcUndef) @@ -6732,12 +6937,67 @@ Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & ti extrinsics = Transform::fromString(_ui->lineEdit_odom_sensor_extrinsics->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString()); timeOffset = _ui->doubleSpinBox_odom_sensor_time_offset->value()/1000.0; scaleFactor = (float)_ui->doubleSpinBox_odom_sensor_scale_factor->value(); + waitTime = _ui->doubleSpinBox_odom_sensor_wait_time->value()/1000.0; return createCamera(driver, _ui->lineEdit_odomSourceDevice->text(), _ui->lineEdit_odom_sensor_path_calibration->text(), false, true, true, false); } return 0; } +Lidar * PreferencesDialog::createLidar() +{ + Lidar * lidar = 0; + Src driver = getLidarSourceDriver(); + if(driver == kSrcLidarVLP16) + { + Transform localTransform = Transform::fromString(_ui->lineEdit_lidar_local_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString()); + if(localTransform.isNull()) + { + UWARN("Failed to parse lidar local transfor string \"%s\"!", + _ui->lineEdit_lidar_local_transform->text().toStdString().c_str()); + localTransform = Transform::getIdentity(); + } + if(!_ui->lineEdit_vlp16_pcap_path->text().isEmpty()) + { + // PCAP mode + lidar = new LidarVLP16( + _ui->lineEdit_vlp16_pcap_path->text().toStdString(), + _ui->checkBox_vlp16_organized->isChecked(), + _ui->checkBox_vlp16_stamp_last->isChecked(), + this->getGeneralInputRate(), + localTransform); + } + else + { + // Connect to sensor + + lidar = new LidarVLP16( + boost::asio::ip::address_v4::from_string(uFormat("%ld.%ld.%ld.%ld", + (size_t)_ui->spinBox_vlp16_ip1->value(), + (size_t)_ui->spinBox_vlp16_ip2->value(), + (size_t)_ui->spinBox_vlp16_ip3->value(), + (size_t)_ui->spinBox_vlp16_ip4->value())), + _ui->spinBox_vlp16_port->value(), + _ui->checkBox_vlp16_organized->isChecked(), + _ui->checkBox_vlp16_hostTime->isChecked(), + _ui->checkBox_vlp16_stamp_last->isChecked(), + this->getGeneralInputRate(), + localTransform); + + } + if(!lidar->init()) + { + UWARN("init lidar failed... "); + QMessageBox::warning(this, + tr("RTAB-Map"), + tr("Lidar initialization failed...")); + delete lidar; + lidar = 0; + } + } + return lidar; +} + bool PreferencesDialog::isStatisticsPublished() const { return _ui->groupBox_publishing->isChecked(); @@ -6935,7 +7195,7 @@ void PreferencesDialog::testOdometry() odomViewer->resize(1280, 480+QPushButton().minimumHeight()); odomViewer->registerToEventsManager(); - CameraThread cameraThread(camera, this->getAllParameters()); // take ownership of camera + SensorCaptureThread cameraThread(camera, this->getAllParameters()); // take ownership of camera cameraThread.setMirroringEnabled(isSourceMirroring()); cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value()); @@ -6954,7 +7214,8 @@ void PreferencesDialog::testOdometry() _ui->doubleSpinBox_source_scanVoxelSize->value(), _ui->spinBox_source_scanNormalsK->value(), _ui->doubleSpinBox_source_scanNormalsRadius->value(), - (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value()); + (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value(), + _ui->checkBox_source_scanDeskewing->isChecked()); if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast(camera) == 0) { cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked()); @@ -6973,7 +7234,7 @@ void PreferencesDialog::testOdometry() } } - UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent"); + UEventsManager::createPipe(&cameraThread, &odomThread, "SensorEvent"); if(imuThread) { UEventsManager::createPipe(imuThread, &odomThread, "IMUEvent"); @@ -7011,7 +7272,7 @@ void PreferencesDialog::testCamera() Camera * camera = this->createCamera(); if(camera) { - CameraThread cameraThread(camera, this->getAllParameters()); + SensorCaptureThread cameraThread(camera, this->getAllParameters()); cameraThread.setMirroringEnabled(isSourceMirroring()); cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked()); cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value()); @@ -7030,7 +7291,8 @@ void PreferencesDialog::testCamera() _ui->doubleSpinBox_source_scanVoxelSize->value(), _ui->spinBox_source_scanNormalsK->value(), _ui->doubleSpinBox_source_scanNormalsRadius->value(), - (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value()); + (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value(), + _ui->checkBox_source_scanDeskewing->isChecked()); if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast(camera) == 0) { cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked()); @@ -7048,7 +7310,7 @@ void PreferencesDialog::testCamera() cameraThread.setDistortionModel(_ui->lineEdit_source_distortionModel->text().toStdString()); } } - UEventsManager::createPipe(&cameraThread, window, "CameraEvent"); + UEventsManager::createPipe(&cameraThread, window, "SensorEvent"); cameraThread.start(); window->exec(); @@ -7112,8 +7374,8 @@ void PreferencesDialog::calibrate() _calibrationDialog->setStereoMode(false); // this forces restart _calibrationDialog->setCameraName(QString(camera->getSerial().c_str())+"_rgb"); _calibrationDialog->registerToEventsManager(); - CameraThread cameraThread(camera, this->getAllParameters()); - UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent"); + SensorCaptureThread cameraThread(camera, this->getAllParameters()); + UEventsManager::createPipe(&cameraThread, _calibrationDialog, "SensorEvent"); cameraThread.start(); _calibrationDialog->exec(); _calibrationDialog->unregisterFromEventsManager(); @@ -7140,8 +7402,8 @@ void PreferencesDialog::calibrate() _calibrationDialog->setStereoMode(false); // this forces restart _calibrationDialog->setCameraName(QString(camera->getSerial().c_str())+"_depth"); _calibrationDialog->registerToEventsManager(); - CameraThread cameraThread(camera, this->getAllParameters()); - UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent"); + SensorCaptureThread cameraThread(camera, this->getAllParameters()); + UEventsManager::createPipe(&cameraThread, _calibrationDialog, "SensorEvent"); cameraThread.start(); _calibrationDialog->exec(); _calibrationDialog->unregisterFromEventsManager(); @@ -7184,7 +7446,7 @@ void PreferencesDialog::calibrate() { return; } - SensorData rgbData = camera->takeImage(); + SensorData rgbData = camera->takeData(); UASSERT(rgbData.cameraModels().size() == 1); rgbModel = rgbData.cameraModels()[0]; delete camera; @@ -7193,7 +7455,7 @@ void PreferencesDialog::calibrate() { return; } - SensorData irData = camera->takeImage(); + SensorData irData = camera->takeData(); serial = camera->getSerial(); UASSERT(irData.cameraModels().size() == 1); irModel = irData.cameraModels()[0]; @@ -7269,8 +7531,8 @@ void PreferencesDialog::calibrate() _calibrationDialog->setSavingDirectory(this->getCameraInfoDir()); _calibrationDialog->registerToEventsManager(); - CameraThread cameraThread(camera, this->getAllParameters()); - UEventsManager::createPipe(&cameraThread, _calibrationDialog, "CameraEvent"); + SensorCaptureThread cameraThread(camera, this->getAllParameters()); + UEventsManager::createPipe(&cameraThread, _calibrationDialog, "SensorEvent"); cameraThread.start(); @@ -7313,27 +7575,14 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() } } - Src odomDriver; - if(_ui->comboBox_odom_sensor->currentIndex() == 0) + Src odomDriver = getOdomSourceDriver(); + if(odomDriver == kSrcUndef) { QMessageBox::warning(this, tr("Calibration"), tr("No odometry sensor selected!")); return; } - else if(_ui->comboBox_odom_sensor->currentIndex() == 1) - { - odomDriver = kSrcStereoRealSense2; - } - else if(_ui->comboBox_odom_sensor->currentIndex() == 2) - { - odomDriver = kSrcStereoZed; - } - else - { - UERROR("Odom sensor %d not implemented", _ui->comboBox_odom_sensor->currentIndex()); - return; - } // 3 steps calibration: RGB -> IR -> Extrinsic @@ -7429,7 +7678,7 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() delete camera; return; } - SensorData odomSensorData = camera->takeImage(); + SensorData odomSensorData = camera->takeData(); if(odomSensorData.cameraModels().size() == 1) { odomSensorModel = odomSensorData.cameraModels()[0]; } @@ -7453,7 +7702,7 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() delete camera; return; } - SensorData camData = camera->takeImage(); + SensorData camData = camera->takeData(); serial = camera->getSerial(); if(camData.cameraModels().size() == 1) { cameraModel = camData.cameraModels()[0]; @@ -7530,5 +7779,42 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics() } } +void PreferencesDialog::testLidar() +{ + CameraViewer * window = new CameraViewer(this, this->getAllParameters()); + window->setWindowTitle(tr("Lidar viewer")); + window->setWindowFlags(Qt::Window); + window->resize(1280, 480+QPushButton().minimumHeight()); + window->registerToEventsManager(); + window->setDecimation(1); + + Lidar * lidar = this->createLidar(); + if(lidar) + { + SensorCaptureThread lidarThread(lidar, this->getAllParameters()); + lidarThread.setScanParameters( + _ui->checkBox_source_scanFromDepth->isChecked(), + _ui->spinBox_source_scanDownsampleStep->value(), + _ui->doubleSpinBox_source_scanRangeMin->value(), + _ui->doubleSpinBox_source_scanRangeMax->value(), + _ui->doubleSpinBox_source_scanVoxelSize->value(), + _ui->spinBox_source_scanNormalsK->value(), + _ui->doubleSpinBox_source_scanNormalsRadius->value(), + (float)_ui->doubleSpinBox_source_scanNormalsForceGroundUp->value(), + _ui->checkBox_source_scanDeskewing->isChecked()); + + UEventsManager::createPipe(&lidarThread, window, "SensorEvent"); + + lidarThread.start(); + window->exec(); + delete window; + lidarThread.join(true); + } + else + { + delete window; + } +} + } diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 3dd32d11b5..ceb86a163d 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -341,8 +341,15 @@ + + + LiDAR + + + + @@ -1718,6 +1725,14 @@ Depth AI + + + true + + + Velodyne VLP-16 + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 8cbd62324c..e1d9c32bd5 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -65,7 +65,7 @@ 0 0 713 - 4002 + 4653 @@ -95,7 +95,7 @@ QFrame::Raised - 3 + 5 @@ -306,41 +306,117 @@ Save/Load Settings - + - - - Load settings (*.ini) ... - - - - - - - Save settings (*.ini) ... - - + + + + + Load settings (*.ini) ... + + + + + + + Save settings (*.ini) ... + + + + + + + Reset all settings + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + - - - Reset all settings + + + Presets + + + + + <html><head/><body><p><a href="https://github.com/introlab/rtabmap/tree/master/data"><span style=" text-decoration: underline; color:#0000ff;">Presets</span></a> are applied after resetting to defaults the 3D Rendering panel and Core parameters. You can create your own presets by saving/loading settings above.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + TOF Camera ICP + + + + + + + <html><head/><body><p>e.g., Kinect [v2, For Azure] and L515 cameras. Example <a href="http://official-rtab-map-forum.206.s1.nabble.com/Kinect-For-Azure-L515-ICP-lighting-invariant-mapping-td7187.html"><span style=" text-decoration: underline; color:#0000ff;">here</span></a>.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + LiDAR 3D ICP + + + + + + + e.g., Velodyne, RoboSense and Ouster LiDARs. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + - - - - Qt::Horizontal - - - - 40 - 20 - - - - @@ -2973,1301 +3049,3508 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Source - + - - - - - Source type. Select specific driver below. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> - - - 0 0 0 0 0 0 - - - - - - - - - ... + + + Camera Sensor + + + + + + + + QComboBox::AdjustToContents + + + + RGB-D + + + + + Stereo + + + + + RGB + + + + + Database + + + + + None + + + + + + + + Source type. Select specific driver below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 0 - - - - - - - 100 - 0 - + + 0 - - + + 0 - - - - - - - - - - - false - - - - - - - Hz - - - 1 - - - 100.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - - - - QComboBox::AdjustToContents - - - - RGB-D - - - - - Stereo - - - - - RGB - - - - - Database - - - - - - - - - 0 - 0 - - - - Create Calibration - - - - - - - Equalizes the histogram of grayscale images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - 0 - 0 - - - - Calibrate - - - - - - - Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - QComboBox::AdjustToContents - - - - None - - - - - Histogram - - - - - CLAHE - - - - - - - - Input rate (0 means as fast as possible). - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - 0 - 0 - - - - Test - - - - - - - Calibration files are saved in "camera_info" folder of the working directory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 1 - - - - - - - ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Mirroring mode (flip image horizontally). It has no effect on database source. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Local transform from /base_link to /camera_link. Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Detect features inside camera thread using parameters set in Visual Registration panel. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - - - 1 - - - - - - - RGB-D - - - false - - - false + + 0 - - - - - Grabber for RGB-D devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - QComboBox::AdjustToContents - - - - OpenNI-PCL - - - - - Freenect - - - - - OpenNI-CV - - - - - OpenNI-CV-ASUS - - - - - OpenNI2 - - - - - Freenect2 - - - - - RealSense - - - - - Images - - - - - Kinect for Windows SDK v2 - - - - - RealSense2 - - - - - Kinect for Azure - - - - - - - - Driver. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - 0 - - - - - - - - 0 - 0 - - - - OpenNI - - - - - - - - - - - - - Path to a *.ONI file. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - ... - - - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + Hz + + + 1 + + + 100.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Input rate (0 means as fast as possible). + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> + + + 0 0 0 0 0 0 + + + + + + + Local transform from /base_link to /camera_link. Mouse over the box to show formats. If odometry sensor is enabled, it is the transform to odometry sensor. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Mirroring mode (flip image horizontally). It has no effect on database source. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + + + + + Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size. If depth images are smaller than RGB images, the decimation is first applied on RGB, if the resulting RGB image is still bigger than depth image, the depth is not decimated. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + Histogram + + + + + CLAHE + + + + + + + + Equalizes the histogram of grayscale images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Only RGB images (for RGB-D cameras) or left images (for stereo cameras) are published. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Detect features inside camera thread using parameters set in Visual Registration panel. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + + + 100 + 0 + + + + + - - - - - - - 0 - 0 - - - - OpenNI 2 - - - - - - - - - true - - - - - - - Auto exposure. - - - true - - - - - - - - - - - - - - - - - true - - - - - - - 65535 - - - - - - - Use timestamps and frame IDs from OpenNI2. - - - true - - - - - - - Auto white balance. - - - true - - - - - - - IR-Depth vertical shift. Positive toward up, negative toward down. - - - true - - - - - - - Mirroring. - - - true - - - - - - - - - - false - - - - - - - pix - - - -9999 - - - 9999 - - - - - - - Exposure. - - - true - - - - - - - 1000 - - - 100 - - - - - - - Gain. - - - true - - - - - - - Path to a *.ONI file. - - - true - - - - - - - ... - - - - - - - - - - false - - - - - - - IR-Depth horizontal shift. Positive toward left, negative toward right. - - - true + + + + + + + Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 0 + 0 + + + + Calibrate + + + + + + + Calibration files are saved in "camera_info" folder of the working directory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 0 + 0 + + + + Create Calibration + + + + + + + Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 0 + 0 + + + + Test + + + + + + + + + + 1 + + + + + + + RGB-D + + + false + + + false + + + + + + Grabber for RGB-D devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + QComboBox::AdjustToContents + + + + OpenNI-PCL + + + + + Freenect + + + + + OpenNI-CV + + + + + OpenNI-CV-ASUS + + + + + OpenNI2 + + + + + Freenect2 + + + + + RealSense + + + + + Images + + + + + Kinect for Windows SDK v2 + + + + + RealSense2 + + + + + Kinect for Azure + + + + + + + + Driver. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + 4 + + + + + + + + 0 + 0 + + + OpenNI + + + + + + + + + + + + + Path to a *.ONI file. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + - - + + + + + + Qt::Vertical 20 - 0 + 696 - - - - pix - - - -9999 + + + + + + + + Qt::Vertical - - 9999 + + + 20 + 0 + - + - - - - Depth decimation. + + + + + + + + Qt::Vertical - - true + + + 20 + 0 + - + - - - - - - - 1 - - - 65535 + + + + + + + + + 0 + 0 + + + OpenNI 2 + + + + + + Path to a *.ONI file. + + + true + + + + + + + + + + + + + + ... + + + + + + + Auto white balance. + + + true + + + + + + + + + + true + + + + + + + Auto exposure. + + + true + + + + + + + + + + true + + + + + + + Exposure. + + + true + + + + + + + 65535 + + + + + + + Gain. + + + true + + + + + + + 1000 + + + 100 + + + + + + + Use timestamps and frame IDs from OpenNI2. + + + true + + + + + + + + + + false + + + + + + + Mirroring. + + + true + + + + + + + + + + false + + + + + + + IR-Depth horizontal shift. Positive toward left, negative toward right. + + + true + + + + + + + pix + + + -9999 + + + 9999 + + + + + + + IR-Depth vertical shift. Positive toward up, negative toward down. + + + true + + + + + + + pix + + + -9999 + + + 9999 + + + + + + + Depth decimation. + + + true + + + + + + + + + + 1 + + + 65535 + + + + - - - - - - - - - - 0 - 0 - - - - Freenect2 - - - - - - Depth edge aware filtering. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + 0 + + + Freenect2 + + + + + + Depth edge aware filtering. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 65.000000000000000 + + + 12.000000000000000 + + + + + + + QComboBox::AdjustToContents + + + + Registered Color to Depth SD + + + + + Registered Depth to Color SD (old way) + + + + + Registered Depth to Color HD + + + + + Registered Depth to Color HD (old way) + + + + + IR + Depth + + + + + + + + + + + true + + + + + + + Depth bilateral filtering. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + Min depth. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Format. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 64.000000000000000 + + + 0.100000000000000 + + + 0.300000000000000 + + + + + + + Depth noise filtering. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Max depth. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + Pipeline, any one of "gl cl clkde cuda cudakde cpu". If empty, default one is used. Depending on how libfreenect2 was built, the selected pipeline may not be available. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + - - - - 65.000000000000000 - - - 12.000000000000000 + + + + + + + + + 0 + 0 + + + RealSense + + + + + + QComboBox::AdjustToContents + + + + Best Quality + + + + + Largest Image + + + + + Highest Framerate + + + + + + + + RGB camera source. For fisheye has depth available only if it is calibrated. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + Color + + + + + Infrared + + + + + Fisheye + + + + + + + + Preset for depth stream. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Preset for RGB stream. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Use visual inertial odometry for ZR300. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Depth image scaled to RGB image size. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + QComboBox::AdjustToContents + + + + Best Quality + + + + + Largest Image + + + + + Highest Framerate + + + + + - - - - QComboBox::AdjustToContents + + + + + + + + + 0 + 0 + - - - Registered Color to Depth SD - - - - - Registered Depth to Color SD (old way) - - - - - Registered Depth to Color HD - - - - - Registered Depth to Color HD (old way) - - - - - IR + Depth - - + + RGB-D Images + + + + + + Maximum frames after start position. 0 means publishing all frames. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + For RGBD-SLAM datasets, it is 5. + + + 3 + + + 1.000000000000000 + + + 999999.000000000000000 + + + + + + + + + + + + + + Path to directory containing RGB images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 9999999 + + + + + + + 9999999 + + + + + + + ... + + + + + + + Start position (index). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Depth scale factor (depth = pixel value / factor). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + Path to directory containing depth images. The directory should have the same size has the RGB directory. The depth images should be already registered to RGB images. Assume that UINT16 images are in mm and FLOAT32 images are in m. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - - - - true + + + + + + + + + 0 + 0 + + + Kinect for Windows SDK v2 + + + + + + QComboBox::AdjustToContents + + + + Registered Color to Depth SD + + + + + Registered Depth to Color SD + + + + + Registered Depth to Color HD + + + + + + + + Format. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - Depth bilateral filtering. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + 0 + 0 + + + RealSense2 + + + + + + + + + false + + + + + + + IR emitter enabled + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + IR mode: use IR camera instead of RGB camera. Tracking will be more accurate (field-of-view of the IR camera is larger with less motion blur). Make sure to disable IR emitter. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Use depth image in IR mode (instead of right image). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + pix + + + 9999 + + + + + + + RGB/IR stream width. Set 1280 for L515. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + pix + + + 9999 + + + + + + + RGB/IR stream height. Set 720 for L515. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Hz + + + 999 + + + + + + + RGB/IR stream rate. Set 30 for L515. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + pix + + + 9999 + + + + + + + (L515) Depth stream width. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + pix + + + 9999 + + + + + + + (L515) Depth stream height. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Hz + + + 999 + + + + + + + (L515) Depth stream rate. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Global time sync. This will make sure IMU data is interpolated at image timestamp, otherwise latest received IMU is synchronized to current frame. Set to off if your firmware of your camera cannot synchronize timestamps. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + <html><head/><body><p>D400 Series Visual Presets. See this <a href="https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets"><span style=" text-decoration: underline; color:#0000ff;">page</span></a>.</p></body></html> + + + true + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - - - - true + + + + + + + + + 0 + 0 + + + Kinect for Azure + + + + + + + + + false + + + + + + + + + + + + + + Use MKV file stamps as input rate. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Frames per second + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + + + + + + 0 + 0 + + + + + 5 + + + + + 15 + + + + + 30 + + + + + + + + + 720p + + + + + 1080p + + + + + 1440p + + + + + 1536p + + + + + 2160p + + + + + 3072p + + + + + + + + Path to a *.MKV file. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Use IR for RGB image (may work better in dark areas). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 320x288 (NFOV 2x2BINNED) + + + + + 640x576 (NFOV UNBINNED) + + + + + 512x512 (WFOV 2x2BINNED) + + + + + 1024x1024 (WFOV UNBINNED) + + + + + + + + Depth camera resolution. 2x2 binning mode extends the Z-range in comparison to the corresponding unbinned mode. Binning is done at the cost of lowering image resolution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>RGB camera resolution. Note that when color camera is used (IR mode is not checked below), to <span style=" font-weight:600;">avoid black borders in point clouds</span> generated, set ROI ratios under 3D Rendering settings to &quot;0.05 0.05 0.05 0.05&quot; under Map and Odom columns.</p></body></html> + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - Min depth. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + + + + + + + + + + + + + Stereo + + + false + + + false + + + + + + Grabber for stereo devices (i.e., Bumblebee2, RealSense2, DepthAI, Zed cameras). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + QComboBox::AdjustToContents + + + + DC1394 + - - - - Format. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + FlyCapture2 + - - - - 64.000000000000000 - - - 0.100000000000000 - - - 0.300000000000000 - - + + + Images + - - - - Depth noise filtering. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + Video + - - - - Max depth. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + ZED sdk + - - - - - - - true - - + + + Usb camera + - - - - Pipeline, any one of "gl cl clkde cuda cudakde cpu". If empty, default one is used. Depending on how libfreenect2 was built, the selected pipeline may not be available. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + Tara Camera + - - - - - - + + + RealSense2 + - - - - - - - - - - - - 0 - 0 - - - - RealSense - - - - - - QComboBox::AdjustToContents - - - - Best Quality - - - - - Largest Image - - - - - Highest Framerate - - - - - - - - Preset for depth stream. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + MyntEyeS + - - - - Preset for RGB stream. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + ZED Open Capture + - - - - QComboBox::AdjustToContents - - - - Best Quality - - - - - Largest Image - - - - - Highest Framerate - - - + + + DepthAI + - - - - Use visual inertial odometry for ZR300. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + Driver. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Auto exposure compensation between left and right images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + 4 + + + + + + + + + + + + + + 0 + 0 + + + Stereo Images + + + + + + ... + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + Path to directory containing left images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Path to directory containing right images. The directory should have the same size has the left images directory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ... + + + + + + + Start position (index). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 9999999 + + + + + + + Maximum frames after start position. 0 means publishing all frames. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 9999999 + + + + - - - - - - - false + + + + + + + + + 0 + 0 + + + Stereo Video (*.avi, *.mp4) + + + + + + ... + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + Side-by-Side (SBS) video or left video. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Right video. Should be empty if a SBS video is set above. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ... + + + + - - - - - - - false + + + + + + + + + 0 + 0 + + + Zed sdk + + + + + + Self-Calibration. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + NONE + + + + + PERFORMANCE + + + + + QUALITY + + + + + ULTRA + + + + + NEURAL + + + + + + + + [SDK3] Texture confidence. Threshold to reject depth values based on their textureness confidence. A lower value means more confidence and precision (but less density). An upper value reduces filtering (more density, less certainty.). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Resolution. Not used when a SVO file is used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Depth confidence. Filtering value for the disparity map (and by extension the depth map). A lower value means more confidence and precision (but less density), an upper value reduces the filtering (more density, less certainty). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Sensing mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 100 + + + + + + + 100 + + + + + + + + + ... + + + + + + + + + + + + + + + + + + + + + + + Quality. If NONE, the disparity is not computed on the GPU, but on CPU using OpenCV (see StereoBM panel for parameters). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Path to a *.SVO file. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + STANDARD + + + + + FILL + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + HD2K + + + + + HD1080 + + + + + HD1200 + + + + + HD720 + + + + + SVGA + + + + + VGA + + + + + AUTO + + + + + - - - - Depth image scaled to RGB image size. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + 0 + 0 + + + Usb Camera + + + + + + Optional right device ID. It should be -1 if the stereo camera streams side-by-side images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + + + + + Stream width (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Stream height (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + -1 + + + + + + + 0 + + + 10000 + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + - - - - QComboBox::AdjustToContents + + + + + + + + + + + + + + + 0 + 0 + - - - Color - - - - - Infrared - - - - - Fisheye - - + + MYNT EYE S + + + + + + + + + + + + + Compute depth image with MYNT EYE S SDK. Ignored if images are not rectified from MYNT EYE S SDK. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Rectify images using MYNT EYE S SDK. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + 240 + + + 120 + + + + + + + 48 + + + 24 + + + + + + + + + + true + + + + + + + Image gain [0-48], valid if manual-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Image brightness [0-240], valid if manual-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Auto-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Image contrast [0-254], valid if manual-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 254 + + + 116 + + + + + + + IR control [0-160]. 0 means that the IR pattern is not projected. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 160 + + + 116 + + + + - - - - RGB camera source. For fisheye has depth available only if it is calibrated. + + + + Qt::Vertical - - true + + + 20 + 0 + - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + 0 + 0 + + + Zed Open Capture + + + + + + + HD2K + + + + + HD1080 + + + + + HD720 + + + + + VGA + + + + + + + + Resolution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - + + Qt::Vertical 20 - 40 + 0 - - - - - - - - - - 0 - 0 - - - - RGB-D Images - - - - - - Path to directory containing RGB images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... + + + + + + + 0 + 0 + + + DepthAI + + + + + + 0 + + + QComboBox::AdjustToContents + + + + Disabled + + + + + 64 pixels + + + + + 96 pixels + + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + 720p + + + + + 800p + + + + + 400p + + + + + 480p + + + + + 1200p + + + + + + + + -1.000000000000000 + + + 1.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 + + + + + + + <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> + + + IR laser dot projector intensity. 0 to turn off. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + None + + + + + GFTT + + + + + SuperPoint + + + + + HF-Net + + + + + + + + Use the translation information from the board design data (not the calibration data). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Output mode. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + On-device feature detector. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + Stereo + + + + + Mono+Depth + + + + + Color+Depth + + + + + + + + <html><head/><body><p>Disparity range increased from 0-95 to 0-190, combined from full resolution and downscaled images.</p></body></html> + + + Extended disparity. Suitable for short range objects. Currently incompatible with sub-pixel disparity. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 255 + + + 200 + + + + + + + -1 + + + 255 + + + 5 + + + + + + + + + + + + + + Disparity confidence threshold (0-255). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> + + + IR flood light intensity. 0 to turn off. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Path to MyriadX blob file. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.000000000000000 + + + + + + + <html><head/><body><p>Computes disparity with sub-pixel interpolation (3 fractional bits by default). </p></body></html> + + + Subpixel mode: number of fractional bits. Suitable for long range. Currently incompatible with extended disparity. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Resolution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + QComboBox::AdjustToContents + + + + Disabled + + + + + Subpixel 3 + + + + + Subpixel 4 + + + + + Subpixel 5 + + + + + + + + + + + + + + + Computes and combines disparities in both L-R and R-L directions, and combine them. + + + Disparity left-right check threshold. Set -1 to turn off. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + 1.000000000000000 + + + 0.050000000000000 + + + 0.000000000000000 + + + + + + + On some high distortion lenses, and/or due to rectification (image rotated) invalid areas may appear even with alpha=0, in these cases alpha < 0.0 helps removing invalid areas. + + + Free scaling parameter between 0 (when all the pixels in the undistorted image are valid) and 1 (when all the source image pixels are retained in the undistorted image). If alpha is set to -1, old rectification policy is used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p> * Matching pixel by pixel for N disparities.</p><p> * Matching every 2nd pixel for M disparitites.</p><p> * Matching every 4th pixel for T disparities.</p><p> * In case of 96 disparities: N=48, M=32, T=16.</p><p> * This way the search range is extended to 176 disparities, by sparse matching.</p><p> * Note: when enabling this flag only depth map will be affected, disparity map is not.</p></body></html> + + + Disparity companding using sparse matching. Currently incompatible with extended disparity. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + 100 + 0 + + + + + + + + + + + + + IMU published + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - Depth scale factor (depth = pixel value / factor). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + Qt::Vertical - - - - - - ... + + + 20 + 0 + - + - - - - - - + + + + + + + + + + + + + + + RGB + + + false + + + false + + + + + + + + Source type. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + + Usb camera + - - - - - - + + + Images + - - - - Path to directory containing depth images. The directory should have the same size has the RGB directory. The depth images should be already registered to RGB images. Assume that UINT16 images are in mm and FLOAT32 images are in m. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - + + + Video file + - - - - For RGBD-SLAM datasets, it is 5. - - - 3 - - - 1.000000000000000 - - - 999999.000000000000000 - + + + + + + Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + 0 + + + + + + + USB Camera + + + + + + 0 + + + 10000 + + + + + + + Stream height (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Stream width (0 for default resolution). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 10000 + + + + - - + + Qt::Vertical @@ -4279,108 +6562,127 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Start position (index). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 9999999 - + + + + + + + + Images Dataset + + + + + + false + + + + + + + Start position (index). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 999999999 + + + + + + + ... + + + + + + + Maximum frames after start position. 0 means publishing all frames. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 999999999 + + + + - - - - Maximum frames after start position. 0 means publishing all frames. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + Qt::Vertical - - - - - - 9999999 + + + 0 + 0 + - + - - - - - - - - - - 0 - 0 - - - - Kinect for Windows SDK v2 - - - - - - QComboBox::AdjustToContents - - - - Registered Color to Depth SD - - - - - Registered Depth to Color SD - - - - - Registered Depth to Color HD - - + + + + + + Video (AVI) + + + + + + false + + + + + + + ... + + + + - - - - Format. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - + + Qt::Vertical - 20 + 0 0 @@ -4388,3540 +6690,1966 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - Qt::Vertical - - - - 20 - 283 - - - - - - - - - - - - - 0 - 0 - - - - RealSense2 - - - - - - Global time sync. This will make sure IMU data is interpolated at image timestamp, otherwise latest received IMU is synchronized to current frame. Set to off if your firmware of your camera cannot synchronize timestamps. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - Hz - - - 999 - - - - - - - (L515) Depth stream width. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - pix - - - 9999 - - - - - - - RGB/IR stream width. Set 1280 for L515. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Hz - - - 999 - - - - - - - Use depth image in IR mode (instead of right image). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - pix - - - 9999 - - - - - - - RGB/IR stream rate. Set 30 for L515. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - <html><head/><body><p>D400 Series Visual Presets. See this <a href="https://github.com/IntelRealSense/librealsense/wiki/D400-Series-Visual-Presets"><span style=" text-decoration: underline; color:#0000ff;">page</span></a>.</p></body></html> - - - true - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - IR mode: use IR camera instead of RGB camera. Tracking will be more accurate (field-of-view of the IR camera is larger with less motion blur). Make sure to disable IR emitter. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - (L515) Depth stream height. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - false - - - - - - - pix - - - 9999 - - - - - - - (L515) Depth stream rate. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - IR emitter enabled - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - RGB/IR stream height. Set 720 for L515. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - pix - - - 9999 - - - - - - - - - - - - - - - 0 - 0 - - - - Kinect for Azure - - - - - - - - - false - - - - - - - - - - - - - - Use MKV file stamps as input rate. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Frames per second - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - - 0 - 0 - - - - - 5 - - - - - 15 - - - - - 30 - - - - - - - - - 720p - - - - - 1080p - - - - - 1440p - - - - - 1536p - - - - - 2160p - - - - - 3072p - - - - - - - - Path to a *.MKV file. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use IR for RGB image (may work better in dark areas). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - 320x288 (NFOV 2x2BINNED) - - - - - 640x576 (NFOV UNBINNED) - - - - - 512x512 (WFOV 2x2BINNED) - - - - - 1024x1024 (WFOV UNBINNED) - - - - - - - - Depth camera resolution. 2x2 binning mode extends the Z-range in comparison to the corresponding unbinned mode. Binning is done at the cost of lowering image resolution. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>RGB camera resolution. Note that when color camera is used (IR mode is not checked below), to <span style=" font-weight:600;">avoid black borders in point clouds</span> generated, set ROI ratios under 3D Rendering settings to &quot;0.05 0.05 0.05 0.05&quot; under Map and Odom columns.</p></body></html> - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - + + + - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - - Stereo - - - false - - - false - - + + - - - Grabber for stereo devices (i.e., Bumblebee2, Zed camera). + + + Database - - true + + false - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + false - - - - - - - - QComboBox::AdjustToContents - - - - DC1394 + + + + + 0 - - - - FlyCapture2 + + 99999 - - - - Images + + + + + + 0 - - - - Video + + 99999 - - + + + + + + + - ZED sdk + Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - Usb camera + - - + + + + - Tara Camera + Ignore goals saved in the database. - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + - RealSense2 + + + + + + + + Open database viewer - - - MyntEyeS + - - + + + :/images/mag_glass.png:/images/mag_glass.png + + + + + - ZED Open Capture + - - + + + + - DepthAI + Ignore goal delay. - - - - - - - Driver. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Auto exposure compensation between left and right images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - - 10 - - - - - - - - - - - - - - 0 - 0 - - - - Stereo Images - - - - - - ... - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - Path to directory containing left images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Path to directory containing right images. The directory should have the same size has the left images directory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - ... - - - - - - - Start position (index). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 9999999 - - - - - - - Maximum frames after start position. 0 means publishing all frames. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 9999999 - - - - - - - - - - - - - - - 0 - 0 - - - - Stereo Video (*.avi, *.mp4) - - - - - - ... - - - - - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - Side-by-Side (SBS) video or left video. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Right video. Should be empty if a SBS video is set above. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - ... - - - - - - - - - - - - - - - 0 - 0 - - - - Zed sdk - - - - - - Self-Calibration. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - NONE - - - - - PERFORMANCE - - - - - QUALITY - - - - - ULTRA - - - - - NEURAL - - - - - - - - [SDK3] Texture confidence. Threshold to reject depth values based on their textureness confidence. A lower value means more confidence and precision (but less density). An upper value reduces filtering (more density, less certainty.). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Resolution. Not used when a SVO file is used. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Depth confidence. Filtering value for the disparity map (and by extension the depth map). A lower value means more confidence and precision (but less density), an upper value reduces the filtering (more density, less certainty). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Sensing mode. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 100 - - - - - - - 100 - - - - - - - - - ... - - - - - - - - - - - - - - - - - - - - - - - Quality. If NONE, the disparity is not computed on the GPU, but on CPU using OpenCV (see StereoBM panel for parameters). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Path to a *.SVO file. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - STANDARD - - - - - FILL - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - HD2K - - - - - HD1080 - - - - - HD1200 - - - - - HD720 - - - - - SVGA - - - - - VGA - - - - - AUTO - - - - - - - - - - - - - - - - 0 - 0 - - - - Usb Camera - - - - - - Optional right device ID. It should be -1 if the stereo camera streams side-by-side images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - 10000 - - - - - - - Stream width (0 for default resolution). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Stream height (0 for default resolution). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - -1 - - - - - - - 0 - - - 10000 - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Ignore features. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Use database stamps as input rate. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Stop position (node ID) is the last node to process. If 0, all nodes after start position are published. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ... + + + + + + + + + + + + + + + + + + + + + Ignore landmarks. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + -1 + + + 9999 + + + + + + + Start position (node ID). + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Ignore priors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 1375 + + + + + + + + + + + + Directory of images (optional settings) + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + + Ground truth file. Select the correct format below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - - - - - - - - - 0 - 0 - - - - MYNT EYE S - - - - - - - - - - - - - Compute depth image with MYNT EYE S SDK. Ignored if images are not rectified from MYNT EYE S SDK. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Rectify images using MYNT EYE S SDK. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - 240 - - - 120 - - - - - - - 48 - - - 24 - - - - - - - - - - true - - - - - - - Image gain [0-48], valid if manual-exposure. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Image brightness [0-240], valid if manual-exposure. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Auto-exposure. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Image contrast [0-254], valid if manual-exposure. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 254 - - - 116 - - - - - - - IR control [0-160]. 0 means that the IR pattern is not projected. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 160 - - - 116 - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + + Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - - - - - - 0 - 0 - - - - Zed Open Capture - - - - - - - HD2K - - - - - HD1080 - - - - - HD720 - - - - - VGA - - - - - - - - Resolution. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + + Bayer mode. For convenience, if the images are bayered. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - - - - - - 0 - 0 - - - - DepthAI - - - - - - 0 - - - QComboBox::AdjustToContents - - - - Disabled - - - - - 64 pixels - - - - - 96 pixels - - - - - - - - 0 - - - QComboBox::AdjustToContents - - - - 720p - - - - - 800p - - - - - 400p - - - - - 480p - - - - - 1200p - - - - - - - - -1.000000000000000 - - - 1.000000000000000 - - - 1.000000000000000 - - - 0.000000000000000 - - - - - - - <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> - - - IR laser dot projector intensity. 0 to turn off. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - QComboBox::AdjustToContents - - - - None - - - - - GFTT - - - - - SuperPoint - - - - - HF-Net - - - - - - - - Use the translation information from the board design data (not the calibration data). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Output mode. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - On-device feature detector. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - QComboBox::AdjustToContents - - - - Stereo - - - - - Mono+Depth - - - - - Color+Depth - - - - - - - - <html><head/><body><p>Disparity range increased from 0-95 to 0-190, combined from full resolution and downscaled images.</p></body></html> - - - Extended disparity. Suitable for short range objects. Currently incompatible with sub-pixel disparity. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - 255 - - - 200 - - - - - - - -1 - - - 255 - - - 5 - - - - - - - - - - - - - - Disparity confidence threshold (0-255). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>Intensity on range 0 to 1, that will determine brightness.</p></body></html> - - - IR flood light intensity. 0 to turn off. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Path to MyriadX blob file. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 1.000000000000000 - - - 0.050000000000000 - - - 0.000000000000000 - - - - - - - <html><head/><body><p>Computes disparity with sub-pixel interpolation (3 fractional bits by default). </p></body></html> - - - Subpixel mode: number of fractional bits. Suitable for long range. Currently incompatible with extended disparity. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Resolution. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - QComboBox::AdjustToContents - - - - Disabled - - - - - Subpixel 3 - - - - - Subpixel 4 - - - - - Subpixel 5 - - - - - - - - - - - - - - - Computes and combines disparities in both L-R and R-L directions, and combine them. - - - Disparity left-right check threshold. Set -1 to turn off. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - 1.000000000000000 - - - 0.050000000000000 - - - 0.000000000000000 - - - - - - - On some high distortion lenses, and/or due to rectification (image rotated) invalid areas may appear even with alpha=0, in these cases alpha < 0.0 helps removing invalid areas. - - - Free scaling parameter between 0 (when all the pixels in the undistorted image are valid) and 1 (when all the source image pixels are retained in the undistorted image). If alpha is set to -1, old rectification policy is used. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p> * Matching pixel by pixel for N disparities.</p><p> * Matching every 2nd pixel for M disparitites.</p><p> * Matching every 4th pixel for T disparities.</p><p> * In case of 96 disparities: N=48, M=32, T=16.</p><p> * This way the search range is extended to 176 disparities, by sparse matching.</p><p> * Note: when enabling this flag only depth map will be affected, disparity map is not.</p></body></html> - - - Disparity companding using sparse matching. Currently incompatible with extended disparity. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - ... - - - - - - - - 100 - 0 - - - - - - - - - - - - - IMU published - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + + ... + - - - - - - - - - - - - - RGB - - - false - - - false - - - - - - + + + + + + + + + + + + Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Odometry file. Select the correct format below. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Use file names as timestamps. Format is epoch time. Example: "1305031102.175304.png". + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Maximum laser scan points. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Synchronize capture rate with timestamps. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if "Use file names as timestamps" above is checked. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + 99999999 + + + + + + + <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> + + + QComboBox::AdjustToContents + + - Source type. + Raw - - true + + + + RGBD-SLAM (motion capture) - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + KITTI - - - - - - 0 + + + + TORO - - - Usb camera - - - - - Images - - - - - Video file - - - - - - + + - Rectify images. If checked, the images will be rectified using the calibration file (if its name is set above). If not checked, we assume that images are already rectified. + g2o - - true + + + + NewCollege - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + Malaga Urban - - - - + + - + St Lucia Stereo - - - - - - - - 0 - - - - - - - USB Camera - - - - - - 0 - - - 10000 - - - - - - - Stream height (0 for default resolution). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Stream width (0 for default resolution). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - 10000 - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - + + + + Karlsruhe + + + + + EuRoC MAV + + + + + RGBD-SLAM + + + + + RGBD-SLAM + ID + + - - - - - - Images Dataset - - - - - - false - - - - - - - Start position (index). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - 999999999 - - - - - - - ... - - - - - - - Maximum frames after start position. 0 means publishing all frames. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - 0 - - - 999999999 - - - - - - - - - - Qt::Vertical - - - - 0 - 0 - - - - - + + + + + ... + - - - - - - Video (AVI) - - - - - - false - - - - - - - ... - - - - - - - - - - Qt::Vertical - - - - 0 - 0 - - - - - + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0<br/>KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0</p></body></html> + + + 0 0 0 0 0 0 + - - - - - - - - - - - - - Database - - - false - - - false - - - - - - 0 - - - 99999 - - - - - - - 0 - - - 99999 - - - - - - - - - - Ignore odometry saved in the database, so if RGB-D SLAM is activated, odometry will be recomputed. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Ignore goals saved in the database. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - - - - Open database viewer - - - - - - - :/images/mag_glass.png:/images/mag_glass.png - - - - - - - - - - - - - - Ignore goal delay. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Ignore features. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use database stamps as input rate. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Stop position (node ID) is the last node to process. If 0, all nodes after start position are published. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Camera index. If the database contains multi-camera data, you can choose which camera to use. -1 means that all cameras are streamed. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - ... - - - - - - - - - - - - - - - - - - - - - Ignore landmarks. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - -1 - - - 9999 - - - - - - - Start position (node ID). - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - If the database contains stereo data, generate disparity image and convert it to depth. The resulting output is a RGB-D image instead of stereo images. Dense disparity parameters can be found under StereoBM tab. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Ignore priors. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - - - - - - - Directory of images (optional settings) - - - - 0 - - - 0 - - - 0 - - - 0 - - - - - - - Ground truth file. Select the correct format below. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Bayer mode. For convenience, if the images are bayered. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Odometry file. Select the correct format below. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Use file names as timestamps. Format is epoch time. Example: "1305031102.175304.png". - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Maximum laser scan points. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Synchronize capture rate with timestamps. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if "Use file names as timestamps" above is checked. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 99999999 - - - - - - - <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> - - - QComboBox::AdjustToContents - - - - Raw - - - - - RGBD-SLAM (motion capture) - - - - - KITTI - - - - - TORO - - - - - g2o - - - - - NewCollege - - - - - Malaga Urban - - - - - St Lucia Stereo - - - - - Karlsruhe - - - - - EuRoC MAV - - - - - RGBD-SLAM - - - - - RGBD-SLAM + ID - - - - - - - - ... - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0<br/>KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0</p></body></html> - - - 0 0 0 0 0 0 - - - - - - - Local transform from /base_link to /scan_link. Mouse over the box to show formats. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - QComboBox::AdjustToContents - - - - Disabled - - - - - BG - - - - - GB - - - - - RG - - - - - GR - - - - - - - - Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> - - - QComboBox::AdjustToContents - - - - Raw - - - - - RGBD-SLAM (motion capture) - - - - - KITTI - - - - - TORO - - - - - g2o - - - - - NewCollege - - - - - Malaga Urban - - - - - St Lucia Stereo - - - - - Karlsruhe - - - - - EuRoC MAV + + + + + Local transform from /base_link to /laser_link. Mouse over the box to show formats. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + QComboBox::AdjustToContents + + + + Disabled + + + + + BG + + + + + GB + + + + + RG + + + + + GR + + + + + + + + Odometry format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>Raw Format (3 values): x y z<br/>Raw Format (6 values): x y z roll pitch yaw<br/>Raw Format (7 values): x y z qx qy qz qw<br/>Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>RGBD-SLAM (stamp tx ty tz qx qy qz qw)<br/>RGBD-SLAM + ID (stamp id tx ty tz qx qy qz qw)<br/>KITTI (stamp + 12 values transform)<br/>TORO<br/>g2o<br/>NewCollege (stamp x y)<br/>Malaga Urban (GPS)<br/>St Lucia Stereo (INS)<br/>EuRoC MAV (stamp,tx,ty,tz,qw,qx,qy,qz...)</p></body></html> + + + QComboBox::AdjustToContents + + + + Raw + + + + + RGBD-SLAM (motion capture) + + + + + KITTI + + + + + TORO + + + + + g2o + + + + + NewCollege + + + + + Malaga Urban + + + + + St Lucia Stereo + + + + + Karlsruhe + + + + + EuRoC MAV + + + + + RGBD-SLAM + + + + + RGBD-SLAM + ID + + + + + + + + ... + + + + + + + Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + s + + + 4 + + + 9.990000000000000 + + + 0.010000000000000 + + + 0.020000000000000 + + + + + + + Local transform from /base_link to /imu_link. Mouse over the box to show formats. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Path to file containing optional IMU data (*.csv [EuRoC format]). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ... + + + + + + + + + + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0</p></body></html> + + + 0 0 0 0 0 0 + + + + + + + IMU Rate. To synchronize capture rate with IMU timestamps, set to 0. This can be set a little over the actual IMU rate to keep up with camera capture rate if images are dropped by odometry. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + EuRoC: 200 Hz -> 250 Hz + + + 99999999 + + + + + + + Load config file for each frame. For calibration, global calibration path above should be empty. Config files should be in the same directory than RGB frames and they should have the same name than the corresponding frame file. Currently supporting 3DScannerApp for iOS export config format (JSON, intrinsics, pose and stamp) and RTAB-Map calibration file format. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + Generate depth image from laser scan - - - - RGBD-SLAM + + true - - - - RGBD-SLAM + ID + + false - - - - - - - ... - - - - - - - Max time difference between data and corresponding pose for format with stamps. If delay is over this threshold, the pose won't be set on data loaded. This is used when odometry and/or ground truth files are set. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - s - - - 4 - - - 9.990000000000000 - - - 0.010000000000000 - - - 0.020000000000000 - - - - - - - Local transform from /base_link to /imu_link. Mouse over the box to show formats. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Path to file containing optional IMU data (*.csv [EuRoC format]). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ... - - - - - - - - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>EuRoC: /base_link to /imu = 0 0 1 0 -1 0 1 0 0</p></body></html> - - - 0 0 0 0 0 0 - - - - - - - IMU Rate. To synchronize capture rate with IMU timestamps, set to 0. This can be set a little over the actual IMU rate to keep up with camera capture rate if images are dropped by odometry. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - EuRoC: 200 Hz -> 250 Hz - - - 99999999 - - - - - - - Load config file for each frame. For calibration, global calibration path above should be empty. Config files should be in the same directory than RGB frames and they should have the same name than the corresponding frame file. Currently supporting 3DScannerApp for iOS export config format (JSON, intrinsics, pose and stamp) and RTAB-Map calibration file format. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - + + + + + Interpolate depth values to fill holes + + + true + + + + + + Vertically + + + true + + + + + + + Horizontally + + + + + + + + + + Fill holes from the image border. + + + + + + + + - + - Generate depth image from laser scan - - - true + Depth Image Filtering - - false - - + + + + + + + ... + + + + + + + + 100 + 0 + + + + + + + + + + + Open database viewer + + + + + + + :/images/mag_glass.png:/images/mag_glass.png + + + + + + + Distortion model (output from depth calibration). + + + true + + + + + - + - Interpolate depth values to fill holes + Bilateral Filtering of the Depth Image true - - - + + + + + + + + 4 + + + 0.000100000000000 + + + 1.000000000000000 + + + 0.001000000000000 + + + 0.005000000000000 + + + + + - Vertically + Standard deviation of the Gaussian for the intensity difference. Set the standard deviation of the Gaussian used to control how much an adjacent pixel is downweighted because of the intensity difference (depth in our case). - + true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - + + + + + + + 1 + + + 0.100000000000000 + + + 100.000000000000000 + + + 1.000000000000000 + + + 5.000000000000000 + + + + + - Horizontally + Size of the Gaussian bilateral filter window to use. Set the standard deviation of the Gaussian used by the bilateral filter for the spatial neighborhood/window. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - Fill holes from the image border. - - - - - - - - - - Odometry Sensor - - - - - - Time offset between camera and odometry sensors. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - QComboBox::AdjustToContents - - - - None - - - - - RealSense2 (T265) - - - - - ZED sdk - - - - - - - - - - - 4 - - - 0.000100000000000 - - - 999.000000000000000 - - - 0.100000000000000 - - - 1.000000000000000 - - - - - - - Driver. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Extrinsics between pose frame and camera's left lens (without optical rotation). Default extrinsics match the 3D printed bracket <a href=" https://www.intelrealsense.com/depth-and-tracking-combined-get-started/"><span style=" text-decoration: underline; color:#0000ff;">here</span></a> for T265+D400 setup. (<a href="https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl"><span style=" text-decoration: underline; color:#0000ff;">stl</span></a>). Not used if camera and odometry sensors are the same sensor. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Calibrate extrinsics between odometry sensor and camera. Both sensors should be already calibrated. See Calibrate button above to calibrate them individually. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + IMU Filtering + + + + + + + + None + + + + + Madgwick Filter + + + + + Complementary Filter + + + + + + + + IMU filtering strategy used to estimate the orientation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + + + + false + + + + + + + Convert IMU in base frame before filtering. This can help to initialize correctly the yaw. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Publish inter IMU messages from the camera. IMU received between images will be published as separate topic. Orientation overridden (if any) if IMU filtering is used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 0 + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + Madgwick Filter + + + + + + + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + Gain. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + Zeta. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + Complementary Filter + + + + + + Do bias estimation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Gain acc. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + Bias alpha. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 4 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.010000000000000 + + + + + + + + + + + + + + Do adaptive gain. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + - - - - - - ... + + + + + + + Visual Odometry Sensor + + + + + + + + QComboBox::AdjustToContents + + + None + + + + + RealSense2 (T265) + + + + + ZED sdk + + - - - - - 100 - 0 - - + + - + Driver. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p><br/></p></body></html> - - - 0.009 0.021 0.027 0.000 -0.018 0.005 - - - - - - - ms - - - 1 - - - -999.000000000000000 - - - 999.000000000000000 - - - - - - - Use as ground truth. The odometry poses will be saved in ground truth field of sensor data instead of being used as odometry. The actual odometry will be computed by rtabmap using the camera sensor. This can be useful to compare rtabmap odometry versus odometry sensor, and to estimate a scale factor between the sensors (using "rtabmap-report -- scale") that can be used above. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - 0 - 0 - - - - Calibrate Extrinsics - - - - - - - Scale factor between camera and odometry sensor. This factor is multiplied to translation components of each pose in the odometry trajectory. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - + + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Calibration file path (*.yaml) for the visual odometry sensor. If empty, the GUID of the camera is used (for those having one). Only required to calibrate extrinsics. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + ... + + + + + + + + 100 + 0 + + + + + + + + + + + + + Calibrate extrinsics between visual odometry sensor and camera. Both sensors should be already calibrated. See Calibrate button above to calibrate them individually. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + ms + + + 1 + + + -999.000000000000000 + + + 999.000000000000000 + + + + + + + ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Time offset between camera and visual odometry sensors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + 4 + + + 0.000100000000000 + + + 999.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + Scale factor between camera and visual odometry sensor. This factor is multiplied to translation components of each pose in the odometry trajectory. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Use as ground truth. The odometry poses will be saved in ground truth field of sensor data instead of being used as odometry. The actual odometry will be computed by rtabmap using the camera sensor. This can be useful to compare rtabmap odometry versus odometry sensor, and to estimate a scale factor between the sensors (using "rtabmap-report -- scale") that can be used above. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>Extrinsics between pose frame and camera's left lens (without optical rotation). Default extrinsics match the 3D printed bracket <a href=" https://www.intelrealsense.com/depth-and-tracking-combined-get-started/"><span style=" text-decoration: underline; color:#0000ff;">here</span></a> for T265+D400 setup. (<a href="https://github.com/IntelRealSense/realsense-ros/blob/occupancy-mapping/realsense2_camera/meshes/mount_t265_d435.stl"><span style=" text-decoration: underline; color:#0000ff;">stl</span></a>). Not used if camera and visual odometry sensors are the same sensor.</p></body></html> + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + 0 + 0 + + + + Calibrate Extrinsics + + + + + + + + + + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p><br/></p></body></html> + + + 0.009 0.021 0.027 0.000 -0.018 0.005 + + + + + + + Maximum wait time to get the pose for latest data captured. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + ms + + + 0 + + + 0.000000000000000 + + + 1000.000000000000000 + + + 100.000000000000000 + + + + - - + + + + + + + LiDAR Sensor + + + false + + + false + + + + - Calibration file path (*.yaml) for the odometry sensor. If empty, the GUID of the camera is used (for those having one). Only required to calibrate extrinsics. + If you want to use ICP registration, the sensor data should have laser scan. Laser scans can be created from the depth images (see option below) or loaded from the source selected. These parameters can be used to reduce the point cloud size directly in the capturing thread. true - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - Depth Image Filtering - - - - - - - ... + + + + + QComboBox::AdjustToContents + + + None + + + + + VLP-16 + + - - - - - 100 - 0 - + + + + LiDAR odometry will update the camera local transform to match its real pose at the LiDAR stamp. - + Grabber for LiDAR devices (e.g., Velodyne). The capture will be synchronized with LiDAR frame rate. If a camera driver is set, images will be added to LiDAR capture. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - Open database viewer - + + - - - :/images/mag_glass.png:/images/mag_glass.png - - - + + - Distortion model (output from depth calibration). + Generate laser scan from depth image. true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + - - - Bilateral Filtering of the Depth Image - - - true - - - - + + + + 0 + + + 0 + + + 0 + + + 0 + + + - + m + + + 9999.000000000000000 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + m - 4 + 3 + + + 0.010000000000000 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> - 0.000100000000000 + 1 - 1.000000000000000 + 99999999 - - 0.001000000000000 + + + + + + Minimum range, filter points below that range. 0 means disabled. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Lidar deskewing. The input scan should have time channel and "Visual Odometry Sensor" should be used. If only IMU is available through the camera, enable publishing of inter IMU under "IMU filtering" above and enable deskewing in Odometry parameters instead. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Search radius for normals computation (0=disabled). Useful if the ICP registration approach is point to plane. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Voxel size for uniform sampling. Note that it is done after downsampling. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 9999.000000000000000 + + + + + + + <html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 0 0 0 0<br/>KITTI: /base_link to /color_camera = 0 -0.06 0 0 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1.67 0 0 0<br/>KITTI: /base_footprint to /color_camera = 0 -0.06 1.67 0 0 0</p><p>EuRoC MAV: /base_link to /cam0 = T_BS*T_SC0 = 0.999661 0.0257743 -0.00375625 0.00981073 -0.0257154 0.999557 0.0149672 0.064677 0.00414035 -0.0148655 0.999881 -0.0216401</p></body></html> + + + 0 0 0 0 0 0 + + + + + + + Local transform from /base_link to /laser_link. Mouse over the box to show formats. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Maximum range, filter points above that range. 0 means disabled. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + 0 + + + 99999999 - 0.005000000000000 + 0 - - + + - Standard deviation of the Gaussian for the intensity difference. Set the standard deviation of the Gaussian used to control how much an adjacent pixel is downweighted because of the intensity difference (depth in our case). + K nearest neighbors for normals computation (0=disabled, 20 can be a good default value). Useful if the ICP registration approach is point to plane. true @@ -7932,31 +8660,52 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - + + - - 1 + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - 0.100000000000000 + + 0.010000000000000 + + + + + + + Force ground normals to be all upward (e.g., 0.8). Useful to make sure Velodyne's scans on ground have all normals upward. 0 means disabled. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> - 100.000000000000000 - - 1.000000000000000 - - 5.000000000000000 + + 0.010000000000000 - - + + - Size of the Gaussian bilateral filter window to use. Set the standard deviation of the Gaussian used by the bilateral filter for the spatial neighborhood/window. + Downsample step size for laser scans. If your laser scans are created from depth images, the step is used as decimation of the depth images. true @@ -7966,285 +8715,139 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - - - - - - IMU Filtering - - - - - - - - - None - - - - - Madgwick Filter + + + + + 0 + 0 + - - - Complementary Filter + Test - - - - - - - IMU filtering strategy used to estimate the orientation. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - - - - Publish inter IMU messages from the camera. IMU received between images will be published as separate topic. IMU filtering strategy will be ignored. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Convert IMU in base frame before filtering. This can help to initialize correctly the yaw. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false - - - - + + + + - + - 2 + 1 - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - Madgwick Filter - - - - - - - - - 4 - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.010000000000000 - - - - - - - Gain. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - 4 - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.010000000000000 - - - - - - - Zeta. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - + + - + Qt::Vertical 20 - 0 + 255 - - + + - + - Complementary Filter + VLP16 - - - - - Do bias estimation. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - + - - - Gain acc. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - - - - 4 - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.010000000000000 - - + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 255 + + + 192 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 255 + + + 168 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 250 + + + 1 + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 250 + + + 201 + + + + - - + + + + <html><head/><body><p>If the lidar is not connected to a GPS, the lidar's clock may be not in sync with the host computer's clock. By enabling this, as soon as a packet is received, it will be stamped with host time &quot;now&quot;. Note that it affects only the final scan timsestamp, not the individual relative timestamp for each point. It is recommended to enable this if you combine it with other sensors above and it is not connected to a GPS.</p></body></html> + - Bias alpha. + Use host time. See tooltip for more info. true @@ -8254,39 +8857,27 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - + + + - - 4 - - - 0.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - 0.010000000000000 + + true - - + + - + ... - - + + - Do adaptive gain. + IP Address. true @@ -8296,253 +8887,126 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - - - - Laser scans - - - false - - - false - - - - - - If you want to use ICP registration, the sensor data should have laser scan. Laser scans can be created from the depth images (see option below) or loaded from the source selected. These parameters can be used to reduce the point cloud size directly in the capturing thread. - - - true - - - - - - - - - m - - - 9999.000000000000000 - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 0 - - - 99999999 - - - 0 - - - - - - - Downsample step size for laser scans. If your laser scans are created from depth images, the step is used as decimation of the depth images. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - m - - - 3 - - - 0.010000000000000 - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 1 - - - 99999999 - - - - - - - Voxel size for uniform sampling. Note that it is done after downsampling. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - K nearest neighbors for normals computation (0=disabled, 20 can be a good default value). Useful if the ICP registration approach is point to plane. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Search radius for normals computation (0=disabled). Useful if the ICP registration approach is point to plane. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 0.010000000000000 - - - - - - - Force ground normals to be all upward (e.g., 0.8). Useful to make sure Velodyne's scans on ground have all normals upward. 0 means disabled. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - m - - - 9999.000000000000000 - - - - - - - Minimum range, filter points below that range. 0 means disabled. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Maximum range, filter points above that range. 0 means disabled. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - Generate laser scan from depth image. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - <html><head/><body><p>KITTI: 130 000 points</p></body></html> - - - 1.000000000000000 - - - 0.010000000000000 - - - - + + + + This can speedup deskewing. + + + Output organized point cloud. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Path to a *.PCAP file. IP/Port/Host time are ignored if PCAP file is used. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + + + + + + + + Port. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + <html><head/><body><p>KITTI: 130 000 points</p></body></html> + + + QAbstractSpinBox::NoButtons + + + 0 + + + 99999 + + + 2368 + + + + + + + <html><head/><body><p>This makes synchronization closer with other sensors above as they will be pulled only after the scan is fully received.</p></body></html> + + + Make the scan timestamp corresponding to timestamp of the last point of the rotation, otherwise it will be the timestamp of the first point of the rotation. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + @@ -14737,10 +15201,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - Guess based on previous motion. + If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). true @@ -14750,10 +15214,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - [Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame. + Data buffer size (0 means inf). true @@ -14763,23 +15227,32 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - [Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + QComboBox::AdjustToContents + + + No filtering + + + + + Kalman filtering + + + + + Particle filtering + + - - + + - Odometry strategy. + Guess based on previous motion. true @@ -14841,30 +15314,149 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - VINS-Fusion + VINS-Fusion + + + + + OpenVINS + + + + + FLOAM + + + + + Open3D + + + + + + + + Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset). When reset, the odometry starts from the last pose computed. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + + + + + + + + 9999 + + + 100 + + + + + + + [Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Visual Registration -> Visual Feature -> Depth as Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Registration approach. Default means the same registration approach used in Motion Estimation panel (same used by the loop closure detector). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + 3 + + + QComboBox::AdjustToContents + + + + Visual - OpenVINS + Geometry (ICP) - FLOAM + Visual + Geometry - Open3D + [Default] - - + + - Align odometry with ground on initialization. + Fill info with data (inliers/outliers features to be shown in Odometry view). true @@ -14874,17 +15466,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - - - - - - + + - If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)). + Pose estimation filtering strategy. true @@ -14894,31 +15479,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + - - - - 999999 - - - - + Test odometry - - + + - Fill info with data (inliers/outliers features to be shown in Odometry view). + Align odometry with ground on initialization. true @@ -14928,10 +15506,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - + + + + 999999 + + + + + - Data buffer size (0 means inf). + [Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame. true @@ -14941,7 +15526,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 1 @@ -14951,63 +15536,27 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset). When reset, the odometry starts from the last pose computed. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If Visual Registration -> Visual Feature -> Depth as Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection. + + + + s - - true + + 3 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 10.000000000000000 - - - - - - QComboBox::AdjustToContents + + 0.100000000000000 - - - No filtering - - - - - Kalman filtering - - - - - Particle filtering - - - - - - - - + + 1.000000000000000 - - + + 1.000000000000000 @@ -15015,21 +15564,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag 0.100000000000000 - 0.000000000000000 - - - - - - - + 0.500000000000000 - - + + - Pose estimation filtering strategy. + Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if a filtering strategy is set or the delay is below the odometry rate. true @@ -15039,30 +15581,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 999999 - - - - - - - 1.000000000000000 - - - 0.100000000000000 - - - 0.500000000000000 - - - - - + + - Registration approach. Default means the same registration approach used in Motion Estimation panel (same used by the loop closure detector). + Odometry strategy. true @@ -15072,37 +15594,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 3 + + + + - - QComboBox::AdjustToContents + + + + + + 999999 - - - Visual - - - - - Geometry (ICP) - - - - - Visual + Geometry - - - - - [Default] - - - + [Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame. @@ -15115,20 +15621,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - 9999 - - - 100 - - - - - + + - Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if a filtering strategy is set or the delay is below the odometry rate. + Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided). true @@ -15138,22 +15634,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - s - - - 3 - - - 10.000000000000000 - - - 0.100000000000000 - - - 1.000000000000000 + + + + @@ -22383,54 +22867,42 @@ Lower the ratio -> higher the precision. - Iterative closest point (ICP) parameters. - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - 3 - - - 1.000000000000000 - - - 0.010000000000000 + Iterative closest point (ICP) parameters. + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + - - - - m - - - 2 - + + 0.000000000000000 - 999.000000000000000 + 1.000000000000000 - 1.000000000000000 + 0.010000000000000 - 0.000000000000000 + 0.700000000000000 - - + + - Max iterations. + Maximum range filtering. Set to 0 to disable. true @@ -22440,45 +22912,23 @@ Lower the ratio -> higher the precision. - - - - - PCL (Point Cloud Library) - - - - - libpointmatcher - - - - - CCCoreLib (CloudCompare) - - - - - - + + 1 - 999999 - - - 1 + 1000 - 1 + 10 - - + + - Outlier ratio used when ICP strategy is not PCL. For libpointmatcher, this parameter set TrimmedDistOutlierFilter / ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the "finalOverlapRatio". + Uniform sampling voxel size. Set to 0 to disable. true @@ -22488,7 +22938,20 @@ Lower the ratio -> higher the precision. - + + + + 1 + + + 1000 + + + 20 + + + + Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are "pcd", "ply" or "vtk". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten. @@ -22501,30 +22964,36 @@ Lower the ratio -> higher the precision. - - - - Max distance for point correspondences. + + + + m - - true + + 3 - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + 0.000000000000000 + + + 0.010000000000000 + + + 0.005000000000000 - - - - + + + + 0.010000000000000 - - + + - Maximum ICP rotation correction accepted (>0). A large rotation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. + Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when PointToPlane is enabled. true @@ -22534,58 +23003,26 @@ Lower the ratio -> higher the precision. - - - - 1 - - - 1000 - - - 20 - - - - + - - - - m - + + 2 - 10.000000000000000 + 1.000000000000000 0.010000000000000 - - 0.200000000000000 - - - - - - - 1 - - - 1000 - - - 10 - - - + + - Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when PointToPlane is enabled. + Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans. true @@ -22595,10 +23032,10 @@ Lower the ratio -> higher the precision. - - + + - Downsampling step of the cloud. Set to 1 to disable. This is done before uniform sampling. + Ratio of matching correspondences to accept the transform. If the maximum laser scans is set, this is the minimum ratio of correspondences on laser scan maximum size to accept the transform. true @@ -22608,10 +23045,10 @@ Lower the ratio -> higher the precision. - - + + - Strategy. + Search radius to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. true @@ -22621,77 +23058,65 @@ Lower the ratio -> higher the precision. - - - - m - - - 3 - - - 0.000000000000000 - - - 0.010000000000000 - - - 0.005000000000000 + + + + - - - - m - - - 6 - + + - 0.000000000000000 + 1 - 1.000000000000000 + 999999 - 0.010000000000000 + 1 - 0.000000000000000 + 1 - - + + - + Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - Reject - - - - - Constrained PointToPoint - - - - - PointToPoint - - + + + + m + + + 2 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.000000000000000 + - - + + - Search radius to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. + Force 4 DoF: Limit ICP to x, y, z and yaw DoF. Available only with libpointmatcher or CCCoreLib strategies. true @@ -22701,10 +23126,10 @@ Lower the ratio -> higher the precision. - - - - 0.000000000000000 + + + + 3 1.000000000000000 @@ -22712,15 +23137,12 @@ Lower the ratio -> higher the precision. 0.010000000000000 - - 0.700000000000000 - - - + + - Force 4 DoF: Limit ICP to x, y, z and yaw DoF. Available only with libpointmatcher or CCCoreLib strategies. + Downsampling step of the cloud. Set to 1 to disable. This is done before uniform sampling. true @@ -22730,10 +23152,10 @@ Lower the ratio -> higher the precision. - - + + - Stuctural complexity strategy. If structural complexity is below the minimum complexity threshold: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept "as is" the transform computed by PointToPoint. + Maximum ICP translation correction accepted (>0). A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. true @@ -22743,23 +23165,51 @@ Lower the ratio -> higher the precision. - - + + + + + 2 + + 0.010000000000000 + 1.000000000000000 + + 0.100000000000000 + + + 0.850000000000000 + + + + + + + 3 + + + 0.001000000000000 + + + 9999.989999999999782 + 0.010000000000000 + + 0.100000000000000 + - - + + - Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution. + Outlier ratio used when ICP strategy is not PCL. For libpointmatcher, this parameter set TrimmedDistOutlierFilter / ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the "finalOverlapRatio". true @@ -22769,10 +23219,17 @@ Lower the ratio -> higher the precision. - - + + - Point to plane ICP. + + + + + + + + Stuctural complexity strategy. If structural complexity is below the minimum complexity threshold: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept "as is" the transform computed by PointToPoint. true @@ -22782,7 +23239,26 @@ Lower the ratio -> higher the precision. - + + + + + Reject + + + + + Constrained PointToPoint + + + + + PointToPoint + + + + + Number of neighbors to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans. @@ -22795,10 +23271,10 @@ Lower the ratio -> higher the precision. - - + + - Ratio of matching correspondences to accept the transform. If the maximum laser scans is set, this is the minimum ratio of correspondences on laser scan maximum size to accept the transform. + Max iterations. true @@ -22808,22 +23284,38 @@ Lower the ratio -> higher the precision. - - + + + + Point to plane ICP. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + - rad + m - 2 + 6 + + + 0.000000000000000 - 3.140000000000000 + 1.000000000000000 0.010000000000000 - 0.780000000000000 + 0.000000000000000 @@ -22840,45 +23332,48 @@ Lower the ratio -> higher the precision. - - - - Maximum ICP translation correction accepted (>0). A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - + + + + + PCL (Point Cloud Library) + + + + + libpointmatcher + + + + + CCCoreLib (CloudCompare) + + - - + + - + rad 2 - - 0.010000000000000 - - 1.000000000000000 + 3.140000000000000 - 0.100000000000000 + 0.010000000000000 - 0.850000000000000 + 0.780000000000000 - - + + - Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans. + Maximum ICP rotation correction accepted (>0). A large rotation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. true @@ -22888,10 +23383,10 @@ Lower the ratio -> higher the precision. - - + + - Maximum range filtering. Set to 0 to disable. + Strategy. true @@ -22901,29 +23396,29 @@ Lower the ratio -> higher the precision. - - + + m 2 - - 0.000000000000000 + + 10.000000000000000 - 1.000000000000000 + 0.010000000000000 - 0.000000000000000 + 0.200000000000000 - - + + - Uniform sampling voxel size. Set to 0 to disable. + Max distance for point correspondences. true @@ -22933,33 +23428,29 @@ Lower the ratio -> higher the precision. - - - - 0.010000000000000 + + + + m - - - - - 3 + 2 - 0.001000000000000 + 0.000000000000000 - 9999.989999999999782 + 999.000000000000000 - 0.010000000000000 + 1.000000000000000 - 0.100000000000000 + 0.000000000000000 - + Reciprocal correspondences. To be a valid correspondence, the corresponding point in target cloud to point in source cloud should be both their closest closest correspondence. @@ -22972,10 +23463,29 @@ Lower the ratio -> higher the precision. - - + + - + Flag to enable filters above: 1="from" cloud only, 2="to" cloud only, 3=both. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 3 + + + 3 diff --git a/package.xml b/package.xml index 926e64f49c..a1e6e949a5 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.21.4 + 0.21.5 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index bf8a4641b3..f95eee8a35 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -22,6 +22,7 @@ ENDIF(OPENCV_NONFREE_FOUND) IF(TARGET rtabmap_gui) ADD_SUBDIRECTORY( CameraRGBD ) + ADD_SUBDIRECTORY( LidarViewer ) ADD_SUBDIRECTORY( DatabaseViewer ) ADD_SUBDIRECTORY( EpipolarGeometry ) ADD_SUBDIRECTORY( OdometryViewer ) diff --git a/tools/Calibration/main.cpp b/tools/Calibration/main.cpp index 5fb7f14659..97557eb774 100644 --- a/tools/Calibration/main.cpp +++ b/tools/Calibration/main.cpp @@ -25,10 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include #include "rtabmap/core/CameraRGB.h" #include "rtabmap/core/CameraRGBD.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/UConversion.h" #include "rtabmap/gui/CalibrationDialog.h" @@ -271,7 +271,7 @@ int main(int argc, char * argv[]) UFATAL("Calibration for driver %d not available.", driver); } - rtabmap::CameraThread * cameraThread = 0; + rtabmap::SensorCaptureThread * cameraThread = 0; if(camera) { @@ -281,7 +281,7 @@ int main(int argc, char * argv[]) delete camera; exit(1); } - cameraThread = new rtabmap::CameraThread(camera); + cameraThread = new rtabmap::SensorCaptureThread(camera); } dialog.registerToEventsManager(); diff --git a/tools/CameraRGBD/main.cpp b/tools/CameraRGBD/main.cpp index 9116bf550f..6ff6d5d025 100644 --- a/tools/CameraRGBD/main.cpp +++ b/tools/CameraRGBD/main.cpp @@ -346,7 +346,7 @@ int main(int argc, char * argv[]) exit(1); } - rtabmap::SensorData data = camera->takeImage(); + rtabmap::SensorData data = camera->takeData(); if (data.imageRaw().empty()) { printf("Cloud not get frame from the camera!\n"); @@ -522,7 +522,7 @@ int main(int argc, char * argv[]) printf("Saved frames %d to \"%s/left\" and \"%s/right\" directories\n", id, stereoSavePath.c_str(), stereoSavePath.c_str()); } ++id; - data = camera->takeImage(); + data = camera->takeData(); } printf("Closing...\n"); if(viewer) diff --git a/tools/DataRecorder/main.cpp b/tools/DataRecorder/main.cpp index 5eb29e0f95..bf76d06bd0 100644 --- a/tools/DataRecorder/main.cpp +++ b/tools/DataRecorder/main.cpp @@ -28,12 +28,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include #include #include -#include +#include +#include #include #include #include @@ -55,7 +55,7 @@ void showUsage() exit(1); } -rtabmap::CameraThread * cam = 0; +rtabmap::SensorCaptureThread * cam = 0; QApplication * app = 0; // catch ctrl-c void sighandler(int sig) @@ -144,7 +144,7 @@ int main (int argc, char * argv[]) return -1; } ParametersMap parameters = dialog.getAllParameters(); - cam = new CameraThread(camera, parameters); + cam = new SensorCaptureThread(camera, parameters); cam->setMirroringEnabled(dialog.isSourceMirroring()); cam->setColorOnly(dialog.isSourceRGBDColorOnly()); cam->setImageDecimation(dialog.getSourceImageDecimation()); diff --git a/tools/EurocDataset/main.cpp b/tools/EurocDataset/main.cpp index 52071e23b2..34c4f1e530 100644 --- a/tools/EurocDataset/main.cpp +++ b/tools/EurocDataset/main.cpp @@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/Graph.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/OdometryEvent.h" @@ -43,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UProcessInfo.h" #include "rtabmap/core/IMUFilter.h" #include +#include #include #include #include @@ -280,7 +280,7 @@ int main(int argc, char * argv[]) // We use CameraThread only to use postUpdate() method Transform baseToImu(0,0,1,0, 0,-1,0,0, 1,0,0,0); - CameraThread cameraThread(new + SensorCaptureThread cameraThread(new CameraStereoImages( pathLeftImages, pathRightImages, @@ -387,9 +387,9 @@ int main(int argc, char * argv[]) UTimer totalTime; UTimer timer; - CameraInfo cameraInfo; + SensorCaptureInfo cameraInfo; UDEBUG(""); - SensorData data = cameraThread.camera()->takeImage(&cameraInfo); + SensorData data = cameraThread.camera()->takeData(&cameraInfo); UDEBUG(""); int iteration = 0; double start = data.stamp(); @@ -559,9 +559,9 @@ int main(int argc, char * argv[]) fflush(stdout); } - cameraInfo = CameraInfo(); + cameraInfo = SensorCaptureInfo(); timer.restart(); - data = cameraThread.camera()->takeImage(&cameraInfo); + data = cameraThread.camera()->takeData(&cameraInfo); } delete odom; printf("Total time=%fs\n", totalTime.ticks()); diff --git a/tools/KittiDataset/main.cpp b/tools/KittiDataset/main.cpp index 7d3ed21175..6ba82e798c 100644 --- a/tools/KittiDataset/main.cpp +++ b/tools/KittiDataset/main.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/CameraStereo.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/Graph.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/OdometryEvent.h" @@ -41,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UProcessInfo.h" #include +#include #include #include @@ -373,7 +373,7 @@ int main(int argc, char * argv[]) false, // assume that images are already rectified 0.0f); } - CameraThread cameraThread(camera, parameters); + SensorCaptureThread cameraThread(camera, parameters); ((CameraImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false); if(exposureCompensation) { @@ -434,8 +434,8 @@ int main(int argc, char * argv[]) UTimer totalTime; UTimer timer; - CameraInfo cameraInfo; - SensorData data = cameraThread.camera()->takeImage(&cameraInfo); + SensorCaptureInfo cameraInfo; + SensorData data = cameraThread.camera()->takeData(&cameraInfo); int iteration = 0; ///////////////////////////// @@ -569,9 +569,9 @@ int main(int argc, char * argv[]) fflush(stdout); } - cameraInfo = CameraInfo(); + cameraInfo = SensorCaptureInfo(); timer.restart(); - data = cameraThread.camera()->takeImage(&cameraInfo); + data = cameraThread.camera()->takeData(&cameraInfo); } delete odom; printf("Total time=%fs\n", totalTime.ticks()); diff --git a/tools/LidarViewer/CMakeLists.txt b/tools/LidarViewer/CMakeLists.txt new file mode 100644 index 0000000000..6350690091 --- /dev/null +++ b/tools/LidarViewer/CMakeLists.txt @@ -0,0 +1,32 @@ + +IF(NOT WITH_QT) + # visualization module required + FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization) +ENDIF(NOT WITH_QT) + +SET(INCLUDE_DIRS + ${PROJECT_BINARY_DIR}/corelib/include + ${PROJECT_SOURCE_DIR}/corelib/include + ${PROJECT_SOURCE_DIR}/utilite/include + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +SET(LIBRARIES + ${OpenCV_LIBRARIES} + ${PCL_LIBRARIES} +) + +add_definitions(${PCL_DEFINITIONS}) + +INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) + +ADD_EXECUTABLE(lidar_viewer main.cpp) +TARGET_LINK_LIBRARIES(lidar_viewer rtabmap_core rtabmap_utilite ${LIBRARIES}) + +SET_TARGET_PROPERTIES( lidar_viewer + PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidar_viewer) + +INSTALL(TARGETS lidar_viewer + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime + BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime) \ No newline at end of file diff --git a/tools/LidarViewer/main.cpp b/tools/LidarViewer/main.cpp new file mode 100644 index 0000000000..0b4afc2d57 --- /dev/null +++ b/tools/LidarViewer/main.cpp @@ -0,0 +1,145 @@ +/* +Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +// Should be first on windows to avoid "WinSock.h has already been included" error +#include +#include + +#include "rtabmap/core/util2d.h" +#include "rtabmap/core/util3d.h" +#include "rtabmap/core/util3d_transforms.h" +#include "rtabmap/utilite/ULogger.h" +#include "rtabmap/utilite/UMath.h" +#include "rtabmap/utilite/UFile.h" +#include "rtabmap/utilite/UDirectory.h" +#include "rtabmap/utilite/UConversion.h" +#include +#include +#include + +#include +#include +#include //fps calculations +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; +using namespace pcl; +using namespace pcl::console; +using namespace pcl::visualization; + +void showUsage() +{ + printf("\nUsage:\n" + "rtabmap-lidar_viewer IP PORT driver\n" + " driver Driver number to use: 0=VLP16 (default IP and port are 192.168.1.201 2368)\n"); + exit(1); +} + +// catch ctrl-c +bool running = true; +void sighandler(int sig) +{ + printf("\nSignal %d caught...\n", sig); + running = false; +} + +int main(int argc, char * argv[]) +{ + ULogger::setType(ULogger::kTypeConsole); + ULogger::setLevel(ULogger::kInfo); + //ULogger::setPrintTime(false); + //ULogger::setPrintWhere(false); + + int driver = 0; + std::string ip; + int port = 2368; + if(argc < 4) + { + showUsage(); + } + else + { + ip = argv[1]; + port = uStr2Int(argv[2]); + driver = atoi(argv[3]); + if(driver < 0 || driver > 0) + { + UERROR("driver should be 0."); + showUsage(); + } + } + printf("Using driver %d (ip=%s port=%d)\n", driver, ip.c_str(), port); + + rtabmap::LidarVLP16 * lidar = 0; + if(driver == 0) + { + lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port); + } + else + { + UFATAL(""); + } + + if(!lidar->init()) + { + printf("Lidar init failed! Please select another driver (see \"--help\").\n"); + delete lidar; + exit(1); + } + + pcl::visualization::CloudViewer * viewer = new pcl::visualization::CloudViewer("cloud"); + + // to catch the ctrl-c + signal(SIGABRT, &sighandler); + signal(SIGTERM, &sighandler); + signal(SIGINT, &sighandler); + + rtabmap::SensorData data = lidar->takeScan(); + while(!data.laserScanRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running) + { + pcl::PointCloud::Ptr cloud = rtabmap::util3d::laserScanToPointCloudI(data.laserScanRaw(), data.laserScanRaw().localTransform()); + viewer->showCloud(cloud, "cloud"); + printf("Scan size: %ld points\n", cloud->size()); + + int c = cv::waitKey(10); // wait 10 ms or for key stroke + if(c == 27) + break; // if ESC, break and quit + + data = lidar->takeScan(); + } + printf("Closing...\n"); + if(viewer) + { + delete viewer; + } + delete lidar; + return 0; +} diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index 1e7a1dedfb..565cc1ce15 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -34,7 +34,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include #include @@ -42,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include void showUsage() { @@ -388,7 +388,7 @@ int main (int argc, char * argv[]) { if(camera->isCalibrated()) { - rtabmap::CameraThread cameraThread(camera, parameters); + rtabmap::SensorCaptureThread cameraThread(camera, parameters); cameraThread.setScanParameters(icp, decimation<1?1:decimation, 0, maxDepth, voxelSize, normalsK, normalsRadius); diff --git a/tools/Reprocess/main.cpp b/tools/Reprocess/main.cpp index efc0e2ec10..71db0d4b41 100644 --- a/tools/Reprocess/main.cpp +++ b/tools/Reprocess/main.cpp @@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include #include #include #include @@ -816,9 +816,9 @@ int main(int argc, char * argv[]) printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str()); std::map globalMapStats; int processed = 0; - CameraInfo info; - SensorData data = dbReader->takeImage(&info); - CameraThread camThread(dbReader, parameters); // take ownership of dbReader + SensorCaptureInfo info; + SensorData data = dbReader->takeData(&info); + SensorCaptureThread camThread(dbReader, parameters); // take ownership of dbReader camThread.setScanParameters(scanFromDepth, scanDecimation, scanRangeMin, scanRangeMax, scanVoxelSize, scanNormalK, scanNormalRadius); if(scanFromDepth) { @@ -849,11 +849,11 @@ int main(int argc, char * argv[]) while(skippedFrames-- > 0) { ++processed; - data = dbReader->takeImage(); + data = dbReader->takeData(); } } - data = dbReader->takeImage(&info); + data = dbReader->takeData(&info); if(scanFromDepth) { data.setLaserScan(LaserScan()); @@ -1099,7 +1099,7 @@ int main(int argc, char * argv[]) while(skippedFrames-- > 0) { processed++; - data = dbReader->takeImage(&info); + data = dbReader->takeData(&info); if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at(0,0)>=9999) { printf("High variance detected, triggering a new map...\n"); @@ -1113,7 +1113,7 @@ int main(int argc, char * argv[]) } } - data = dbReader->takeImage(&info); + data = dbReader->takeData(&info); if(scanFromDepth) { data.setLaserScan(LaserScan()); diff --git a/tools/RgbdDataset/main.cpp b/tools/RgbdDataset/main.cpp index 144cf6796e..02f86fdbf9 100644 --- a/tools/RgbdDataset/main.cpp +++ b/tools/RgbdDataset/main.cpp @@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap/core/Rtabmap.h" #include "rtabmap/core/CameraRGBD.h" -#include "rtabmap/core/CameraThread.h" #include "rtabmap/core/Graph.h" #include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/OdometryEvent.h" @@ -41,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UProcessInfo.h" #include +#include #include #include @@ -200,7 +200,7 @@ int main(int argc, char * argv[]) //parameters.insert(ParametersPair(Parameters::kg2oBaseline(), uNumber2Str(40.0f/model.fx()))); model.save(path); - CameraThread cameraThread(new + SensorCaptureThread cameraThread(new CameraRGBDImages( pathRgbImages, pathDepthImages, @@ -239,8 +239,8 @@ int main(int argc, char * argv[]) UTimer totalTime; UTimer timer; - CameraInfo cameraInfo; - SensorData data = cameraThread.camera()->takeImage(&cameraInfo); + SensorCaptureInfo cameraInfo; + SensorData data = cameraThread.camera()->takeData(&cameraInfo); int iteration = 0; ///////////////////////////// @@ -255,9 +255,9 @@ int main(int argc, char * argv[]) { ++skipCount; - cameraInfo = CameraInfo(); + cameraInfo = SensorCaptureInfo(); timer.restart(); - data = cameraThread.camera()->takeImage(&cameraInfo); + data = cameraThread.camera()->takeData(&cameraInfo); continue; } skipCount = 0; @@ -375,9 +375,9 @@ int main(int argc, char * argv[]) fflush(stdout); } - cameraInfo = CameraInfo(); + cameraInfo = SensorCaptureInfo(); timer.restart(); - data = cameraThread.camera()->takeImage(&cameraInfo); + data = cameraThread.camera()->takeData(&cameraInfo); } delete odom; printf("Total time=%fs\n", totalTime.ticks());